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ABSTRACT 


Extended Kalman filtering is applied as an extension of the Position Location Re- 
porting Svstem (PLRS) to track a moving target in the XY plane. The application uses 
four sets of observables which correspond to inputs from a fused-sensor array where the 
sensors emploved are acoustic, seismic, or radar. The nonlinearities to the Kalman filter 
occur through the measured observables which are: bearings to the target only, ranges 
to the target only, bearings and ranges to the target, and a Doppler-shifted frequency 
accompanied by the bearing to that frequency. The observables are nonlinear in their 
relationships to the Cartesian coordinate states of the filter. 

Filter error covariances are portrayed as error ellipsoids about the latest target esti- 
mate made by the filter. Rotation of the ellipsoids is accomplished to avoid the cross 
correlation of the coordinates. The ellipsoids employed are one standard of deviation in 
the rotated coordinate svstem and correspond to a constant of probability of target lo- 
cation about the latest Kalman target estimate. 

Filtering techniques are evaluated for both stationary and moving observers with 
arbitrarily moving targets. The objective of creating a user-friendly, personal computer 


based tracking algorithm 1s also discussed. 


THESIS DISCLAIMER 


The reader is cautioned that the computer programs developed in this research may 
not have been exercised for all cases of interest. While every effort has been made within 
the time available to ensure that the programs are free of computational or logical er- 
rors, they cannot be considered validated. Any application of these programs without 
additional verification is at the risk of the user. 
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I. INTRODUCTION 


The military principle of combat maneuver, in its most basic sense, relies upon the 
knowledge of the ground commander as to his position and orientation within the com- 
bat arena. The units involved in a joint maneuver must coordinate their positions and 
Orientations with each other in order to ensure maximum cooperation and the successful 
prosecution of the military campaign. Recent history dictates that the knowledge of a 
tactical military unit position has rested in the individual skill of members of the unit 
with a map and lensatic compass, from which a position could be determined. Orien- 
tation of the unit could be controlled through the accurate association of surrounding 
prominent terrain features to the map. Once location and orientation have been estab- 
lished, they must be transmitted to higher headquarters in order to ensure the proper 
adjacent unit coordination. This knowledge, along with tactical intelligence information 
about the position and orientation of local enemv forces allows the ground commander, 
through his staff, to command and coordinate support operations. These operations 
would include indirect fire support, close-air support, deep-air support, and logistics and 
communication support. 

The Position Location Reporting System (PLRS) was created bv the Hughes Cor- 
poration of Los Angeles for the Marine Corps and Army to ensure, or at least improve, 
command knowledge of the positions and orientations of friendly tactical elements 
Within the combat arena [Ref. 1]. This system, however, does not track enemy forces 
forward of the Forward Edge of the Battle Area (FEBA). This can be overcome by the 
use of an extended Kalman filter algorithm designed as the tracking element with inputs 
from a fused sensor array. The sensors employed would be restricted to four different 
tvpes of observables: target bearings, target ranges, both target bearings and target 
ranges, or a Doppler-shifted frequency signal accompanied by the bearing to that signal. 
The Kalman filter algorithm would take the sensor inputs and through a conversion 
process, show observed and estimated target tracks accompanied by a predicted track 
based on the last estimated position of the target. An addition to the Kalman filter al- 
gorithm could then allow for the display of ellipsoids of constant probability (error 
ellipsoids) about the estimated target positions to show an area in which the target is 


located to a definite probability. 


The operation of PLRS will be discussed, at least partially, in the next chapter. The 
accumulated knowledge of the PLRS observables (position and location of friendly 
units) and the observables of the algorithms developed in this thesis could contribute 
greatly to the knowledge that a potential ground tactical commander could gain about 
his situation within the combat arena and serve to aid in the tactical decision making 
process. The knowledge of friendly and enemy forces within a closed combat arena 1s 
essential to the successful prosecution of military objectives. This axiom is best summed 
up by the Chinese philosopher general, Sun Tzu [Ref. 2] who said 

If you know the enemy and know yourself, you need not fear the result of a hundred 
battles. If you know vourself but not the enemy, for every victory gained you will 
also suffer a defeat. If you know neither the enemy nor yourself, you will succumb 
in CVery Daticw 

One can easily see the necessity for accumulated combat maneuver intelligence. 
Since PLRS has no ability to track an enemy force which is forward of the FEBA and 
concentrates solely on friendly units, a potential tracking algorithm 1s developed in an 
attempt to augment the PLRS system. The algorithm is as previously described and 
based on multiple fused sensors providing measured observables to the tracking element 
for processing. Four types of observables were considered: target bearings only, target 
ranges only, both target bearings and target ranges, and a Doppler-shifted frequency 
accompanied bv the bearing to that frequency. These observables are then fed to an ex- 
tended Kalman filter simulation algorithm to produce the estimated and the predicted 
tracks. 

The extended Kalman filter assumes the existence of nonlinearities between the ob- 
servables and the system states. The simulations conducted restrict movement of the 
target to the NY plane and the system states are derived from these dimensions. Non- 
linearities to the filter algorithms occur in all of the observables considered relative to 
the derived system states. An error ellipsoid algorithm is then employed in order to fur- 
ther isolate the target and to attempt to confirm its probable location. The algorithms 
are then combined using a personal computer-based, user-friendly driver routine in order 
to acquire the necessary inputs. The total simulation program is then analyzed for per- 


formance and an assessment of its tracking capabilities provided. 


If, POSITION LOCATION REPORTING SYSTEM 


The PLRS svstem was developed for many different reasons -- among them was the 
ability to protect friendly tactical elements from fire from friendly sources during combat 
situations and during peacetime training exercises. Accidents occur every year at the 
Marine Corps Air Ground Combat Center (MIMCAGCC), Twentynine Palms, California, 
Which are a direct result of a lack of knowledge as to subordinate unit positions by 
command elements of friendly units. This is caused by untimely updating over command 
communication nets. The Combined Arms Exercise (CAX) and the Regimental Firing 
Exercise (FEX) are commonly used to combine actual movement of troops with live fire 
in order to impress upon commanders the importance of proper coordination and to 
increase realism in combat training. All of the movement corridors at MCAGCC are 
considered as live fire areas and, therefore, the positions of all maneuver elements within 
them is considered essential to both personnel safetv and to mission accomplishment. 

The system consists of a containerized master unit composed of generalized com- 
puter, Communications, and command electronics. This unit is designated as the Master 
Unit (MU). [t is responsible for processing all information inputs into the system and 
for the display of unit positions, coordination measures, and control information visu- 
allv. Locally-placed user units are man or machine portable and are designated as User 
Units (UU). The MU sends a radio message to the UU which then computes a time of 
arrival (TOA) for the message and transmits this information back to the MU. The MIU 
then uses the arrival time information from the UU to compute a range to the UL. The 
Ieee clsommancmuis the localsbarometric pressure to the Mache MUL then uses the 
range, bearing, and pressure information to compute a three-dimensional position of the 
UL. Since the MU will not always have line-of-sight radio contact with all of the local 
user units, the system relies on the user units to determine the distances between them- 
selves and other locally-placed user units by computation of a TOA for the transmissions 
of the other units and to report this range information to the MU. Reports are made 
to the MU through a multi-level relay technique using other user units as necessary. 

Within PLRS, the location of each unit is established through a process called 
trilateration. This involves taking the ranges from three other units with previously es- 
tablished positions and using those ranges to calculate the position of the unlocated unit. 


To accomplish this the MU uses four simplified discrete Kalman filters. These filters are 


called the Central Logic Oscillator Control (CLOC) filter to process clock offset and drift 
rate, the Mean Sea Level (MSL) filter to process an offset calibration for the barometric 
pressure measurements reported to it, the Track Review and Correction Estimation 
(TRACE) filter to enter and partially update each UU position and velocity estimate, 
and an altitude filter to process the barometric pressure data in order to establish and 
aid in tracking the vertical positions of the local user units. 

Tracking of the user units is accomplished by the MU using an internal coordinate 
svstem. The internal coordinate system serves as the basis for all coordinate transfor- 
mation processing. This system is a transverse mercator projection with its central 
meridian at the longitude of system center. The system essentially takes the curved sur- 
face of the earth and projects it stereographically onto a flat plane. This is then called 
the Local Transverse Mercator (LIM) system. The LIM system produces a slight 
amount of distortion for long range measurements at distances far from system center. 
[er 3 

To use the LIM svstem, the TOA measurements must be converted to LIM ranges 
by PLRS in real time. This conversion is accomplished in four steps by the MU and is 
illustrated in Figure | on page 5. The four steps are listed as follows: 


1. The conversion of the TOA measurement to a true range measurement. 


ty 


The conceptual movement of the units to an average altitude of zero. 


The projection of the earth onto a flat plane. 


fw 


The movement of the units back to their original altitudes. 


The use of PLRS greatly improves command and control efforts at the 
regimental, brigade level and above. By provision of improved manuever unit movement 
updates and an increase in communications speed, the PLRS concept significantly im- 
proves fire support coordination and logistical support efforts. The improvements gained 
decrease personnel safety risks while allowing for realism in peacetime training, and in- 


creased unit effectiveness in combat. 
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Figure 1. NIU Conversion of TOA Data to LIM Range: From [Ref. 3] 


Il. MULTIPLE GEOMETRY TARGET TRACKING ALGORITHM 


In order to study the problem by simulation, three different types of simulation 
models had to be developed. The sensors, when actually employed in the field, would 
track a real moving target across the ground. Since a real target does not exist, for the 
purposes of this simulation, a model had to be developed. The sensor system must also 
be modeled so that the observables developed from it can be fed to the extended Kalman 
filter algorithm for processing. The extended Kalman filter must be modeled in a com- 
puter algorithm so as to demonstrate its tracking capability. The simulations must then 
be iterated in order to demonstrate target movement and subsequent updating of the 


estimated positions and velocities of the modeled target. 


A. TARGET MODEL 
The motion of the target is restricted to the XY plane. The simulation model uses 
the basic equation of motion from kinematics 


s= s+ T+ 4,7" (3.1) 


This relation 1s applied in both the X and Y directions and over the total observation 


time 
ir aral aaa ney x 1 (3.2) 


Where x,,,, is the number of target track positions desired. Thus, a complete set of target 
positions over a defined time period can be defined. These positions can then serve as 


the basis for the sensor and Kalman tracker algorithms. 


B. SYSTEM MODEL 
The system modeled is that of a ground target moving in the XY plane. Certain 
restrictions were placed on the problem in order to simplify the mathematics. These re- 
Sstrictlons Were: 
e the curvature of the earth is neglected, 
*¢ target and sensor movement are restricted to a flat plane, and 


¢ target course and speed inputs are constant (1.e., step inputs). 


The observed matrix developed from the sensor simulation algorithm contains no noise, 
so that Gaussian noise can be added to the observations before entry into the extended 
Kalman filter algorithm. This supports realism in the simulation in that the observation 
measurements Would contain Gaussian noise. 

This is a linear, time-distance system that can be described with equations of motion 


for constant acceleration in two dimensions. The state space equation is 
Xi = Pike + Ape (3.3) 


where 
xX, = State vector (estimation parameter) 
@, = state transition matrix (describes how the dynamic system states are related) 
A, = system noise coefficient matrix 


qd, = random forcing function. 


When the above conditions and Equation (3.3) are incorporated, and the observable 
system 18 target bearings only, target ranges only, or both target bearings and target 


ranges, the state vector 1s 


When the set of observables is the Doppler-shifted frequency measurement and the as- 


sociated bearing to that frequency, then the state vector is 


58 


Xe = | Y (3.5) 
y 
So 


where f is the target transmission rest frequency. 
When the following model equations are used and the aforementioned problem 
conditions incorporated into them, the system state equation can be expanded in matrix 


form. The model equations are 


a 


Xpyy = Xp t kyl t+ > Ay (3.6) 


2 
Seg = yp Oy T (3.7) 
Vee) Vk +i T+ OyT (3.8) 
Veer =e t eT (3.9) 
Mrgg Sly (3.10) 


Thus for the systems where the state vector is described by Equation (3.4), the system 


state equation can be expressed as 


x a On Ie ian 0 

x 010 Offs 0. |e, ; 

vi Flo oa THe TL o Pp I (3.11) 
y 000 115 @ 


For the svstems where the state vector is defined by Equation (3.5), the system state 


equation can be expressed as 


x Pr x fei 200 seOnmnOnlie 
x 010 x Oy ee On | 
tbe © I W bee | eee) OP a | (3.12) 
y 0001 Olly oe OF OUT SOLE 
i 000 0 TNF O>Oe Oo OBIE 


where f, through f{ are the random forcing functions included to account for random 
changes in speed, direction, or transmission frequency which can occur for a moving 
target. Each of these random forcing functions must be considered independently in or- 


der to ascertain their effect. The forcing functions can be expressed by 


Ff, =A 9» Ye 4) (3.13) 


f=H@an 7 (3.14) 


S3 =f» Mvp 4) (3.15) 
Sa = SalV@ > Vv,» ) (3.16) 
UB) 3) (3.17) 


where Ya, (for heading), y, (for speed), and Vj, (for the transmission rest frequency) are 
considered as random changes to the target. These changes are assumed to be inde- 


pendent, Zero mean, and piecewise constant rates of change. They have variances defined 


as 
a, = Alyy)’ (3.18) 
09 = Elle)’ (3.19) 
of, = ALC) (3.20) 


The system noise process for the target tracking and prediction problem 1s a function 
of the svstem noise coefficient matrix A,, and the random forcing function a, as defined 
for each of the state vectors. For the state vector of Equation (3.4) this is simply the 


target acceleration vector. For the state vector of Equation (3.5) a, is defined by 


The state space representation for the Doppler-shifted frequency observable was devel- 
oped by Mitschang [Ref. 4]. 


C. MEASUREMENT MODEL 

The problem, as studied, considered four types of observables: bearings to the target 
only, ranges to the target only, both bearings and ranges to the target, and the 
Doppler-shifted frequency accompanied bv a bearing measurement to that frequency. 
With recall of the state vectors as defined by Equations (3.4) and (3.5), the measurement 


model for this problem can be developed. The measurement equation 1s 


ak = Hix; + Vy (322) 


where 
z, = the set of measurements 
H, =the observation matrix (noiseless) 
 — tle statemvecton 


v, = associated measurement noise. 


1. Bearings Only Problem 
In this tracking problem, the measurements are lines of bearing as received bv 
sensors located on separate pronunent terrain features. The geometry of the problem is 
shown in Figure 2 on page Il. The problem geometry shows that the relationship of 
the measurements to the system states 1s nonlinear. For this case the measurement 


equation becomies 


Onn = on] Ma = nid) | + Uy (3.23) 
Vir ey, 
where 
d|, = lines of bearing from sensor n at time k 
XuJ, = the X,Y coordinates of themaneemaunmens 
Xnubne = the X,Y coordinates of sensor n at time k 


v, = the bearing measurement noise. 


2. Ranges Only Problem 
In this tracking problem, the measurements are straight-line distances to the 
target gained by ranging sensors located on separate prominent terrain features. The 
problem geometry is shown in Figure 3 on page 12. This geometry, as in the last case, 
shows a nonlinear relationship between the measurements and the system states. The 


measurement equation 1s 
=/ ; ae 3.24 
Tak = V Xie Xnk + Vie — ¥nk De (3.24) 


where r,, = range distances from sensor n at time k. 
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Figure 2. Bearings Only Geometry: Measurement Inputs to the Kalman Filter 
are Restricted to Target Bearings. 
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Figure 3. Ranges Only Geometry: Measurement Inputs to the Kalman Filter are 
Restricted to Target Ranges. 


3. Bearings and Ranges Problem 
The tracking problem now becomes a combination of the first two problems. 
The measurements are defined as in Equation (3.23) for target bearings and Equation 
(3.24) for target ranges. The geometry for this problem is shown in Figure 4 on page 


14. The observation matrix for this problem is now defined as 


(3.25) 


i 


4+. Doppler-Shifted Frequency Problem 
In this tracking problem, the observables are a Doppler-shifted frequency ac- 
companied by a bearing measurement to that frequency. The problem geometry as 
shown in Figure 5 on page 15, along with knowledge of the Doppler equation [Ref. 5], 
shows that the relationship between the measurements and the system states 1s again 


nonlinear. The measurement equation is 


ran] Nik — “nk 
Vik — Vnk 


oa . Gshe es ha 


Xe ie + Vil re 


(3.26) 
= 7 
VX rk + Vip 


where 
vy, = the velocity of wave propagation (speed of light) 
v,, = bearing measurement noise 


Vv, = frequency measurement noise. 


This measurement equation 1s based on a single sensor which is stationary and located 
at the origin. 

The observables in all four measurement cases have been shown to be nonlinear 
in their relationship to the system states. Processing by the extended Kalman filter 1s 
thus required for target tracking and prediction. The operation of the extended Kalman 


filter will be explained in the next chapter. 
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Yigure 4. Bearings and Ranges Geometry: Measurement Inputs to the Kalman 


Filter are Restricted to Target Bearings and Target Ranges. 
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Figure 5. Doppler Frequency Geometry: Measurement Inputs to the Kalman 
Filter are Restricted to a Doppler Shifted Frequency and the Associated 


Frequency Bearing. 
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IV. KALMAN FILTER THEORY 


The process of estimation of the state vector at the current time, with reference to 
all previous measurements, 1s referred to as filtering. An optimal filter optimizes a spe- 
cific performance measurement which is used to approximate the quality of the estimate. 
The Kalman filter is a linear optimal filter which minimizes the mean square estimation 
error between the actual output and the desired output. The filter, in actuality, is a re- 
cursive algorithm for the optimal processing of discrete measurements or observations 
[Ref. 6]. The filter requires an a priori Knowledge of the state estimate and its error 


covariance as Well as the current observation. Svstem equations for the Kalman filter 


are 
Xpry = Pde + Ange (4.1) 

and 
Zp = LAypxy + Ly (4.2) 


A. THE EXTENDED KALMAN FILTER 

From the measurement equations in the last chapter for each of the observable 
cases, one can see that there 1s a nonlinear relationship between the measurements used 
for target tracking and the svstem state variables. The adaptation of the Kalman filter 
to a nonlinear application is termed as the extended Kalman filter. A general discussion 
of the of this tvpe of filter algorithm follows. 


Given system and measurement equations of the form 
Xp = fl&ys k) + BlXes Kx (4.3) 
Zp = Aly, K) + ky (4.4) 


where 
f, g, h are nonlinear functions of the state vector x , 
w, is the plant excitation noise, 


vy, 18 the measurement noise. 
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The plant excitation noise and the measurement noise are assumed as uncorrelated, zero 


mean, and white. That is 
wy = N(O, Q), 
vy = NC, R), 
and 
EL, wy 1 = Orb y 
El ty. 3) 1 = Bedyy 
where 6, 1s the Kronecker delta function such that 
Og=l  (k=)) 


Oyj =O (k # /). 


(4.9) 


(4.10) 


To apply the linear filter Equations (4.1) and (4.2), an expansion of Equations (4.3) 


and (4.4) is conducted about the best estimate of the state at the current time. This ex- 


pansion is accomplished with a Tavlor series and only the first order terms are kept. 


With the expansion. Equation (4.3) now yields 


Xa = Dire + Aywy 








where 
Cf 
Px — OX, x = Xi 
Equation (4.4) now yields 
Zp = AyXy + Ly 
where 
Ch 


ie 


(4.11) 


(4.12) 


(4.13) 


(4.14) 


The variable x, is the estimated value of the state after the &* measurement and the 
variable x’, is the predicted state value before the k* measurement. These can be speci- 


fied more completely by 
xp = flXp_y, k — 1). (4.15) 


The following set of definitions can then be used to derive the linear Kalman filter 


equations used in the simulation. A state error vector can be defined as 
~ A 
Xp = Xp— Xe (4.16) 
A predicted state error vector can then be defined by 
ae Ay 
Xi y = Ly — Xz. (4.17) 


Equations (4.16) and (4.17) allow the definition of the covariance of state error matrix 
Je 


P, = ELE ] | (4.18) 
and the definition of the predicted covariance of state error matrix P’, by 
Pi, = El x'yx's |: (4.19) 
The state excitation matrix Q, , as seen in Equation (4.7), is defined by 
Oy = EL Ap wysny Aj). (4.20) 
The measurement noise covariance matrix R, , as seen in Equation (4.8), is defined by 
Ru = Elvgyy J. (4.21) 


The definitions cited above provide the basis for the construct for the Kalman filter al- 


gorithm specified by the set of equations below [Ref. 6] 


Ppa = OE Py + Qt (4.22) 
Gy = B’ HELGE’ fy + BY (4.23) 
Ey ad ‘ie: a G,H, Py (4.24) 


Slee 1 &) (4.25) 
Z'y = A(x’, ) (4.26) 


= Xu + Glee — XJ (4.27) 


where 
I = the identitv matrix 


G, = the Kalman gain matrix. 


The Q matrix allows for target maneuvering and also accounts for filter model in- 
accuracies. This greatly decreases the discrepancies which would arise between the sys- 
tem characterization, Equations (4.1) and (4.2), and the true action of the system 
physically. As the filter algorithm reaches steady-state conditions, the Q matrix also 
prevents the Kalman gain matrix, G, , from approaching zero by ensuring an amount 


of uncertainty in the predicted covariance of the state error matrix 2’, 


B. LINEARIZATION OF THE OBSERVATION MATRIX 

For purposes of brevity, the H matrix derivations will be done simultaneously for 
the bearings onlv, ranges only, and the bearings and ranges observable cases. These are 
shghtly different, however they are based upon the same mathematical premise. 
Equations (4.4), (4.13), and (4.14) establish the basis for the linearization of the obser- 
vation matrix H. These equations state that the H matrix is a linearization of the non- 
linear function h and is achieved by taking the partial derivative of h with respect to the 
predicted state. 

1. Target-Bearing Measurement Observables Only 

The h function using bearing measurements onlv can be deduced from Equation 

(3.23) as 


— pap t[ ST Fak 
h(x, kK) = tan | re |. (4.28) 


Applying the above linearization method, 


- “| Coys Xnk) | 
c| tan —_—— 
ES vy) 


Crp 


H; = (4.29) 
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where x, can be recalled for Equation (3.4). Simplifving Equation (4.24) vields 
fe Fe Ay2 hy; | 
| Atay Rag fgg Aga |’ 


where 


| | (X14 = Xnx) l| 
Olt eae ea eae ae 
cea 2 Yk —¥nk 


ee 
ni OX» R?, 
A —| (Xix _ Se) 
Cle tan i 
h Vix ifs ay) ; 
a ay 7 
A = (Xz — Xnk) | 
c} tan i ae = 
ie = Ynk) one (Xrp Saale cone) 
3 = = 
CY rk IS, 
a a ae ~ ei) 
Cietan eae ee 
h Vix — Vp wd) ; 
na — =. 
From Equations (4.31) through (4.34) 
hy, = ~- 
nk 
hy, =0 
(x:p = <p) 
hy; = A> 
oop, 
hig _ 


(4.31) 


(4.32) 


(ea) 


(4.36) 


(4.37) 


(4.38) 


where the range estimate squared R?, is 
A 


2 2 2 
Rik = (Xie, e-1) 7 Xng) We, k-1) — Ine) (4.39) 


The second row of the matrix is completed in a similar manner. 
2. Target-Range Measurement Observables Only 
The target-range-measurement-only h function can be deduced from Equation 
(3.24) as 


2 2 
A(Xp0 kK) = V (Xk — Xnk) + Vie — Yak)” - (4.40) 
Applving the linearization method again yields 


tn. ey a 
él vu = sot) 2 hy, ne ] 


CX 


(4.41) 


H, — 
where again, x, is recalled from Equation (3.4). Simplification of Equation (4.36) vields 
Ay Mya hy; | 
ie 4.42) 
: i hyy hy3 hyq \ | 
ere 


A iv 2 2 
al y (X14 ms ae als On =e nk) | V 


Ie = = es (4.43) 
Xie on 
al J (Sry — Je ag Vix i Baer ] 
|) (4.44) 
CXrK 
2 
| Vxw a a) a Vir — Van) ] — (Xp = cee) 
h,3 = a a | ae Se (4.45) 
Jtk Rie 
2 2 
| Vxn — Xng) + re — Fre) ] 
a (4.46) 


oy tk 


Thus from Equations (4.43) through (4.46) 
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<p ee 
fh (4.47) 


Rrk 

hee | (4.48) 

ae = Vix — (4.49) 
nk 

hy = 0 (4.50) 


and as before, the second row can be computed in a similar manner. 
3. Target-Bearing and Range Measurement Observables 
The h functions for the case where the observables are both the target bearings 
and the target ranges are the same as those derived in both of the previous cases. These 


cases, however. are now combined. The corresponding H matrix is 


Ay, Ayn hy3 Mg 
Ay, Ayq hy3 Nya 
hz; h32 N33 Nga 
Nay haz Ag3 Aaa 


Be 
o 
| 


(4.51) 


where the first and third row of the matrix are the first and second rows of the matrix 
defined in Equation (4.30) replaced in a one-for-one correspondence. The second and 
fourth rows of the matrix are the first and second rows of the matrix defined in Equation 
(4.42) replaced one-for-one. 
4. Doppler-Shifted Frequency Observable 
The h function for the Doppler-shifted frequency observable and the associated 
bearing is somewhat more detailed. Using the state vector defined by Equation (3.5) and 


the measurements obtained from Equation (3.26), one can deduce the following shifted 


oe 


Doppler h function 


ae Xp Xz | 
Mik — Vk 


Ay, k) = ce j fl%> 


Application of the linearization process vields 


WT, = Ayy Aya hy3 Aya ys 
| Atay Aagg hry3 Fog Ags | 


where using the notation of Equation (4.15) 














ee 
hy =— K eS ¢) 
Ovi 
C6, 
h ——— = () 
rr fy 
A 2 ‘ , Vv 
fe LM i ee = XY td 
217 @ - 2 NGS. 
O* tk Hoon Lema i) | 
je I 
= a =) kX tk 
0) 


—c CO ©) Pry 
Tae fig eameeny a | 
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See as Vil Vik 
Vy aS ———— 
f2 a 
vi Xk + iK 


(4.53) 


(4.59) 


(4.60) 


2 . 
Cf, =f Xe oe ell 








hy = = (4.61) 
CV tk f eee + yt) 7) 
Of —f'% Vik 
hog = = (4.62) 


A iielebunl a ig 12 
Gay on ee y rk a 


Che — Se 


Cfo for 





hys (4.63) 
Having linearized the observation matrices for all of the observable cases, the linear 


Kalman filter equations can be utilized. 


C. ACQUIRED NOISE PROCESSES (R AND Q MATRICES) 

Calculation of the covariance of state error matrix, P,, and the Kalman gain matrix 
G,, requires the specification of covariance matrices for the associated uncorrelated noise 
processes a, and y, as used in this study. The covariance matrix for the measurement 
noise process ¥, 1s found in Equation (4.8). R, is defined as the state measurement noise 
covariance matrix. This matrix 1s based on the accuracy of the sensors employed and 
accounts for unknown disturbances in the plant model. 

The state excitation matrix Q, represents the system noise process and is a function 


of the system noise coefficient matrix A, and the random forcing function a,. . Thus 
ye 
% i [ApQ’ Ay | (4.64) 
Where Q’, , from Equation (4.20), is defined as 


E(az ‘) El ayyAxx) 


4.65 
Elagayx) E(a2,) i) 


Q', = Elayay ] = 


This matrix is valid for the bearings and ranges observables and allows for any random 
target maneuvers and any inaccuracies in the system model. 

The derivation of the Q matrix for the Doppler observable 1s slightly more complex 
however, and is based on the random disturbances f| through ff as defined bv Equation 
(3.21). Recall that You Vis and yj, Were the random changes to the target and were as- 
sumed as independent, zero mean, and piecewise continuous. The variances for these 


changes can be defined as 
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a, = Ely, } (4.66) 


%,= EL, ] (4.67) 
a7, = Ely, | (4.68) 


. The standard deviations g,, o,, and a, specify typical maneuvering parameters for the 


target. The Q matrix is found by letting 


2 x; Dore -2 2 
Ox =(F-) Oy, + 37%, (4.69) 
a oa Bee ep ce he 4.70 
a, = ( v, ) Oy Xp Ig. ( . ) 
. an, 2 
Oxy =X,V; aoe soe ay (4.71) 
vy 


where the states are evaluated at the current state estimates x, Substitution of the above 


expressions into the Q matrix yields 


(17/2)%02 (T?/2)03 (T?/2)e2; (T*)2)0?, 


0 
3 
qi20 (F)oz (TF /2)o5; (Tat, 0 
O,=| qi3 q23. (T']2)'o5 (T']2)o; 0 (4.72) 
qi4 q24 q34 (To, 0 
qi5 q25 q35 45 Top 


This analysis was done by Mitschang and is found in detailed form in Reference 4. 


D. PROBLEM PARAMETERS 
The following parameters were used in this analysis: 
oy, = 6.283 rad/hr 
g,, = 0.01852 km/hr 
a3, = 0.01096 (rad/min)? 
a? =0.0001852 (km/min’)’. 


For the Doppler observable, the parameters were: 
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0.00017452 rad/sec 
0.01852 km/sec 
o, = 0.0005 Hz/sec. 
With the @ matrix defined by the system considered as in Chapter III, and the Q, R, and 
H matrices defined as above, the Kalman filter Equations (4.22) through (4.27) can be 


employed and the simulations conducted. 
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V. ERROR ELLIPSOIDS 


The position of each unit within the PLRS system is estimated with a certain finite 
degree of uncertainty about the estimate. The associated uncertainty iS expressed in the 
covariance of error matrix P,. This represents the sigma squared error deviation about 
the estimate. The position diagonal terms, P,, and P,;, represent the variances of the es- 
timate in the X and Y directions respectively. The off-diagonal terms of each represent 
covariances, the degree of coordinate coupling, and the orientation of the uncertaintv in 
Ores 1 plane. 

The errors are normally distributed and there exists a rotated coordinate svstem in 
which the orthogonal position components are uncorrelated. This is identical to taking 
the exponent of the joint probability density function and performing a coordinate 
transformation to eliminate the cross terms. 

The exponent for Gaussian random variables 1s 


x? XV r . 
ee + ; (sel) 


3 OY ORG, oe 
where r,, 1s the cross correlation of X and Y. When this expression is set equal to a 
constant, the geometric interpretation is that of an ellipse which specifies a constant 
probability of target location about the estimate. The major and minor anis of the ellipse 
are oriented in the XY coordinate system. In this system a cross correlation between X 
and Y exists. To eliminate this cross correlation, a coordinate transformation is apphed. 
The positional component X is transformed to a new component X’ and the positional 


component Y is transformed to a new component Y’ by 


x’ =xcos@+ysin 6 ie) 
and 
x’ =ycos@—xsin@ (5.3) 
where 
@= + tan” es (5.4) 
Ceo 


7) 


This specifies the variances in the new coordinate system (X’Y’) as 


Pe 2 
Ox+ Oy  cov(xy) 


? ~ ee 
ag ee sa Be 
and 
2 o* aE oy cov(xr) < 
a sin 20 ow 


The relation used to plot the resulting ellipsoid, which defines a constant probability of 
target location about the latest Kalman estimate, results from the transformation. The 


relation is based on Equation (5.1) and is 


2 Ti 
tak. (5.7) 
Oy Oy 


where Kk defines the probability of target location. 

The error ellipsoid routine emploved in this study is based on the above premise and 
plots ellipsoids respresenting a constant probability of target location about the latest 
Kalman estimate and uses that estimate as the ellipsoid center. The program was written 
by Captain Stephen L. Spehn USMC in collaboration with another project. This pro- 
gram plots a single standard of deviation (in the orthogonal coordinates) ellipsoid with 
a Sixty percent probability of target location within the ellipsoid. The major and minor 
axis of the ellipse may also be plotted in terms of the Mahalanobis distance or the dis- 
tance in standards of deviation [Ref. 7]. The ellipsoids are increased in size bv factors of 


as much as sixteen for ease of view. 
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VI. PROGRAM RESULTS 


A. PROGRAM LOGIC 

The tracking problem solution integrates all four types of geometric concepts of 
Chapter III. In order to simulate the problem and to attempt a solution, each of the 
geometric concepts required an individual working subroutine. The subroutines were 
then integrated using a menu-driven, user-interactive driver file. The driver file was de- 
signed to acquire target position and movement information, tracking problem parame- 
ters, and to determine the tvpe of problem to be solved. 

The program presented herein is an attempt to solve the geometric problem of 
tracking an arbitrarily moving target in the XY plane. It was written in PC-Matlab in 
order to use a personal computer-based mathematical algorithm. The system, when 
augmented and improved with combinational logic and mapping data, could be used bv 
a maneuver commander in the combat arena as an additional tool in his assessment of 
the tactical situation. The advantage that could be gained in the knowledge of enemy 
position and intent would greatly aid in the tactical decision-making process. 

Il. PC-Mfatlab Structure 

The personal computer-based Matlab program uses a unique program structure 
that is similar to Fortran. The program, however, uses different terminology in its 
structure when emploving driver routines. The difference in terminology is derived from 
the way the main Matlab driver programs are written. All Matlab execution files are 
named with a .m suffix. Thus, all of the file names for this problem contain that suffix. 
The driver file, marine.m, calls the various subroutines as needed when in operation. The 
programs written for the study of this problem are included in the Appendix. They are 
commented throughout in attempt to help the reader to become familiar with their 
construct with as little effort as possible. 

2. Subroutine Structure 

The program subroutines each isolate one geometric concept in an attempt to 
simulate and solve the tracking problem. Each subroutine is written to simulate the tar- 
get, to simulate the observers (sensors), and to simulate the operation of the extended 
Kalman filter discussed in Chapter IV. The various steps are commented throughout in 
an effort to aid the reader in understanding the program and its structure. The names 


of the variables used in the programs of the Appendix correspond to those of Chapters 
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Iff and IV as much as possible. It was felt that this would aid in the understanding of 


its Operation. 


B. RESULTS 

The results of the program discussed are presented graphically in order to illustrate 
what the maneuver commander would see if such a system were to be employed on the 
battlefield. Since the program tracks an arbitrarily moving target with arbitrary observer 
placement, the target tracks and the observer (sensor) locations were changed with each 
geometric case. The cases will be presented in the following order: target-bearing ob- 
servables only employed, target-range observables only employed, both target-bearing 
and range observables employed, and the Doppler observable with its associated bearing 


EMipiox cas 
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1. Target-Bearing Observables Only 

Results of a tracking problem using only the observed bearings from a fused- 
sensor array are contained in Figure 6 on page 32 through Figure 9 on page 35. These 
figures show outputs at various stages of the program. Figure 6 on page 32 shows the 
actual track of the target, the circles on the diagram, and the noiseless observations, the 
dots. This graph allows for a comparison of the bearing calculations by the program 
against the track as chosen in the simulation. This is necessary as a check in the opera- 
tion of the bearings only subroutine. If the comparison 1s successful, white noise is added 
to the bearing measurements before thev are submitted to the Kalman algorithm. 
Figure 7 on page 33 shows the actual and estimated target tracks as well as the error 
ellipsoids around the Kalman estimates. The simulation iterates and, when in display on 
the screen, one can see the target move. In this case, the movement is from the upper 
left to the lower right. The rotation of the error ellipsiods to avoid coordinate cross 
correlation is clearly evident. The actual positions are again circles, the estimates are 
crosses, and the error ellipsoids are evident from the plot. Figure § on page 34 shows the 
actual. estimated, and predicted positions of the target. The predictions were obtained 
by taking the predicted Kalman state estimate and the current Kalman state estimate 
and performing a subtraction of the velocity values in the X and Y directions and di- 
viding by the observation interval. This produced an estimate of the random forcing 
function a, as defined in Chapter III. The estimate was then used in the linear Kalman 
svstem equation shown in Equation (4.1) to determine predicted positions based on a 
constant acceleration. This is consistent with a second-order Kalman analvsis in each 
direction where the acceleration is the random forcing function. The predicted positions 
are shown as Stars in the diagram. Figure 9 on page 35 shows what the potential combat 
commander would see should the system be employed. The figure is a combination of 
the actual, estimated, and predicted target tracks as well as the error ellipsoids about the 
Kalman estimates. The figure also shows the location of the observers, as do the previ- 
ous figures. These are portrayed as an x in the diagram. Essentially the program was 
constructed to produce results in response to questions a potential maneuver 
commander would ask while engaged in combat operations. These questions are: 

e Where is the enemy ? 


e Where is he going ? 
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Figure 6. Actual and Observed Tracks: Target-Bearing Observables Only. <Ac- 
tual ‘Track - 0, Observed Track - « , Sensor - x. 
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Figure 7. Actual and Estimated Tracks: Target-Bearing Observables Only. Ac- 
tual Track - 0, Estimated Track - + . The Plot Also Contains Error 
Ellipsoids to Aid in Target Location. 
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Yigure 8. Actual, Estimated, and Predicted Tracks: Target-Bearing Observables 
Only. Actual Track - 0, Estimated Track - +, Predicted Track - *. 
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Figure 9. Total Target Track View: Target-Bearing Observables Only. Actual 
Track - 0, Observed Track - « , Sensor - x. Predicted Track - *. 
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2. Target-Range Observables Only 

The results of a tracking problem using only target-range observables are shown 
in Figure 10 on page 37 through Figure 13 on page 40. These figures are in the same 
order as the ones in the bearings only case. The target track and observer locations have 
been changed in order to demonstrate the versatility of the program. Figure 10 on page 
37 shows the noiseless observations compared to the actual track of the target as chosen 
in the simulation. Again, if the comparison is successful, Gaussian noise is added before 
filtering. Figure 11 on page 38 shows the actual and estimated tracks of the target as 
well as the error ellipsoids about the Kalman estimates. Figure 12 on page 39 shows the 
actual, estimated and predicted target tracks. When the display is on screen, one can 
observe the target movement, which in this case would be from left to mght across the 
diagram. Since the observers are nearly colinear with the first estimate, the error ellipsoid 
shows a substantial bearing error while showing a small range error. This decreases as 
the target moves away from the observers because the range directions are more accu- 
rately determined. Figure 13 on page 40 1s a combination of the previous figures which 
would be used to provide information about the enemy for use 1n the tactical decision 
making process. 

3. Target-Bearing and Range Observables 

The results of a problem emploving both target-bearing and range observables 
are shown in Figure 14 on page 41 through Figure 17 on page 44. The figures are again 
in the same order as the previous two cases to allow comparison. Figure 14 on page 41 
shows the actual target track and the noiseless observations for this case. If the obser- 
vations favorably compare with the actual positions, white noise is added and the noisv 
observations filtered by the Kalman algorithm. Figure 15 on page 42 shows the actual 
and estimated tracks with the error ellipsoids added. The rotation of the ellipsoids to 
avoid cross correlation of the coordinate variables 1s particularly evident in this case. 
Figure 16 on page 43 shows the actual, estimated, and predicted tracks for the target. 
The target movement in this case was from the lower right to the upper left. Figure 17 
on page 44 is a combination of the previous three and would be the result of the program 


when viewed in the field. Again, it provides answers to relevant command questions. 
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Figure 10. Actual and Observed Tracks: Target-Range Observables Only. Ac- 


tual Track - o, Observed Track - « , Sensor - x. 
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Figure 11. Actual and Estimated Tracks: Target-Range Observables Only. Ac- 
tual Track - o, Estimated Track - + . The Plot Also Contains Error 
Ellipsoids to Aid in Target Location. 
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Figure 12. Actual, Estimated, and Predicted Tracks: Target-Range Observables 
Only. Actual Track - 0, Estimated Track - +, Predicted Track - *. 
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Figure 13. Total Target Track View: Target-Range Observables Only. Actual 
Track - 0, Observed Track - « , Sensor - x. Predicted Track - *. 
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Figure 14. Actual and Observed Tracks: Target-Bearing and Range Observables. 
Actual Track - 0, Observed Track - « , Sensor - x. 
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Figure 15. Actual and Estimated Tracks: Target-Bearing and Range Observables. 


Actual Track - 0, Estimated Track - + . The Plot Also Contains Error 
Ellipsoids to Aid in Target Location. 
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Figure 16. Actual, Estimated, and Predicted Tracks: Target-Bearing and Range 
Observables. Actual Track - 0, Estimated Track - +, Predicted Track 
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Figure 17. Total Target Track View: Target-Bearing and Range Observables. 
Actual Track - 0, Observed Track - « , Sensor - x. Predicted Track - 
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4. Doppler-Frequency Observable 

The results of a problem where a Doppler-shifted frequency and the associated 
bearing to that frequency are emploved are shown in Figure 18 on page 46 through 
Figure 21 on page 49. These are based on a system employing one sensor, which is sta- 
tionary and located at the origin. Figure 18 on page 46 shows the actual track positions 
compared to the target observations. If the observations compare to the actual posi- 
tions, white noise is added and the observables sent to the Kalman algorithm for proc- 
essing. Figure 19 on page 47 shows the actual and estimated tracks with the error 
ellipsoids to aid in defining target position. Figure 20 on page 48 shows the actual, es- 
timated , and predicted target tracks. Figure 21 on page 49 is again a combination of the 
previous three graphs and would be the one seen in the field. The target movement in 


this case was from upper left to lower right. 
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Figure 18. Actual and Observed Tracks: Doppler Frequency Observable. Actual 
Track - 0, Observed Track - « , Sensor - x. 
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Figure 19. Actual and Estimated Tracks: Doppler Frequency Observable. Ac- 
tual Track - 0, Estimated Track - + . The Plot Also Contains Error 
Ellipsoids to Aid in Target Location. 
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Figure 20. Actual, Estimated, and Predicted Tracks: Doppler Frequency Ob- 
servable. Actual Track - 0, Estimated Track - +, Predicted Track - *. 
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Figure 21. Total Target Track View: Doppler Frequency Observable. Actual 
Track - 0, Observed Track - « , Sensor - x. Predicted Track - *. 
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VII. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 

The target tracking algorithm developed here is both personal computer-based and 
user-friendly. This would aid in field employment where the system would be of most 
use to the maneuver commander. With improvements in the tracking algorithm pre- 
sented, or on one similiarly based, real-time tracking could be accomplished. These im- 
provements will be subsequently discussed. With real-time tracking, indirect supporting 
arms or direct air could engage the target with a reasonable chance of success. If the 
Orientation of the target were known, optimal damage could then be inflicted upon it. 
Hence, the development of a working fused-sensor system tracking algorithm would 
place forward reconnaissance personnel at intelligent risk when seeking combat maneu- 


ver intelligence far forward of the FEBA. 


B. FUTURE STUDY POSSIBILITIES 

Future study regarding improvements in the algorithm developed is quite extensive. 
Topics which would facilitate the program's usefulness include incorporation of the ac- 
celeration terms in the X and Y directions and the application of the Z coordinate and 
its velocity and acceleration terms. PLRS uses time division multiple access (IT DMA) for 
unit communications transmissions. A delay studv should be done so as to account for 
a worst-case analvsis of the effect that transmission delays would have on the employ- 


ment of the Kalman tracker or its target update capability. 


C. PROGRAM IMPROVEMENTS 

Improved program structure can be achieved by employment of the principles used 
by Baheti [Ref. 8]. The Baheti algorithm requires one quarter of the memory capacity 
and fewer computations than the algorithm as developed here, but does not adhere to 
conventional control methods. The algorithm herein presented was written in PC Matlab 
which is a matrix manipulation program. It will require updating as improvements in the 
Matlab algorithm are developed. 

Since optimal tracking of a ground target would be futile in the fact that the ground 
target motion would be unpredictable, another possible program improvement could be 
researched. The Defense Mapping Agency has worldwide computer mapping data which 


can be displayed on a computer terminal. It may be possible to incorporate this data in 
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a logic algorithm structure so as to determine the target’s tendencies to augment the 
optimal tracking scenario. Once the logical choices are made, the potential maneuver 


commander could employ logic as well as optimal control to thwart his adversary. 


APPENDIX TARGET TRACKING SIMULATION PROGRAMS 


A. PROGRAM DISCUSSION 

The programs contained herein were written to simulate the various aspects of the 
target tracking problem heretofore discussed. They are written in PC-Matlab and hence, 
are denoted by a .m suffix to identifv them as control or execution files. A caveat should 
be issued at this point. This set of programs was written in PC-Matlab version 3.5 which 
contains some newer function files not previously available. One should check the ver- 
sion of PC-Matlab emploved before attempting tracking simulations. If the function files 
necessary to run these programs are not present, an error will result. The programs are 
conimented throughout in the hope that they will aid the user in understanding their 
operation. 

The main driver file for the set is titled marinem. [The subroutineswane: idles 
brgbrg.m. rngrng.m. brgrng.m. and doppler.m. The file errellip.m is called bv each of the 
subroutines as a function file. The function file reshape.m, is also necessary for the op- 
eration of marine.m. It 1s contained for reference. These programs should not be con- 


sidered as validated or free of logical or arithmetic error. 


B. PROGRAM LISTING 


% JOHN A. HUCKS I] 

7% St LLE NAME ee hee! 

vo CONTROL FILE FOR MULTIPLE GEOMETRY EXTENDED KALMAN FILTER TARGET 
% TRACKING SIMULATION PROGRAM. 

7 FILES NECESSARY FOR THE OPERATION OF THIS PROGRAM ARE: 

70 BRGBRG. M 


70 RNGRAG. M 

70 ERGRNG. M 

70 DOLE Leah 

70 ERRELULIP ad (COURTESY OF STEPHEN L. SPEHN) 
70 RESHAPE. M (FUNCTION FILE) 


% THIS PROGRAM DRIVES OTHER MATLAB M (EXECUTION) FILES WHICH SIMULATE 
% TARGET TRACKING BASED ON VARIOUS OBSERVABLES, THESE BEING BEARINGS, 
% RANGES, BEARINGS AND RANGES, OR A DOPPLER SHIFTED FREQUENCY. THE 

7% PROGRAMS ARE BASED ON FUSED MULTIPLE OBSERVERS THAT ARE EITHER 

7 STATIONARY OR MOVING. 


% PROGRAM SETUP 
clear hold off ,etpoloct lil clon cue 


title ieee TARGET TRACKING SIMULATOR'; 
tit he ane ee (OPTIMAL) 


"Say 
tu 


titleline2 = ' NAVAL POSTGRADUATE SCHOOL MSEE THESIS ; 
titleline3 = ' CAPTAIN JOHN A. HUCKS II USMC DEC. 1989 ; 


oma he DISPLAY 


spe | (dS 

Giese. (); 

dase. ): 

dsp (+): 

Gisp( sd; 

Gisp( +); 

Gieep ds; 

Gaispt  ); 

daisp( |); 

disp( 7: 

disp(' '); 
disp(titleline); 
Gasp (titlelinel); 
eeepc titieline2); 
eaiso  titileline3); 
pause 


vo OBSERVER INPUT ROUTINE 


enc 

Bsp( _); 

disp(’ —); 

S536) (ala 

dasoG. ' ): 

Asa ENTER THE FIRST OBSERVERS INITIAL PARAMETERS: '); 

disp(' 3 

Gasp id); 

disp( xfol=[ x % observer x position (km) 
disp(' VX % velocity x direction (km/hr) ) 
disp(' y ie observer y direction (km)') 
disp(' vy J; *% velocity y direction (km/hr)') 
caisn(  —); 


disp( ‘YOU MAY ENTER THIS: IN THE FOLLOWING FORMAT: [x vx y vy] ‘); 
xfols = input( Sse, = IR 

evalc{ xtole= ‘,xfols,';']); 

rol) = reshape(xfol, eel): 


elc 

Gisp(  ); 

dasp( (+); 

disp(' OF 

Gisp( ~ ): 

Ate ENTER THE SECOND OBSERVERS INITIAL PARAMETERS: ' ); 

disp(' ‘); 

dasp(' “); 

disp(' MEo2. = (ax % observer x position (km)') 
disp(' Vx % velocity x direction (km/hr)') 
disp(’ y Jeopsenvermy rcirection (km) 
disp(' my is % velocity y direction (km/hr)') 
disp(' ' 

disp(' YOU MAY ENTER THIS IN THE FOLLOWING FORMAT: lea vy): 
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xfo2s = input ( xfo2 = ieee: 

eval({” xfo2"=" ,xfo2S 75 ier 

xfo2 = reshape(xfo2,4,1); 

Cie 

disp@ae: 

dispar ENTER OBSERVER MOVEMENT SPEED: '); 

dis o(@amee 

disp(' spd % observer speed in km/hr ') 
disp(' 3 

daisy (sae 

spds = input(' spd = 3. S')3 

eval({ spd = ',spds, ; |). 

Cake 

disp = >. 

disp(' ENTER DESIRED NUMBER OF OBSERVATIONS: '); 

disp( Be 

disp(' kmax % observation number ') 
qdisp@ ase) 

clalsyol( 2 


Fa PLEASE MAKE THIS GREATER THAN 5 SO THAT THE KALMAN GAINS' ) 
disp( ‘CAN STABILIZE. ") 


kmaxs = input (— kmax = Se 

eval({ 'kmax = ‘',kmaxs,';']); 

cle 

diss@ ie: 

disp(' ENTER DESIRED NUMBER OF PREDICTIONS: '); 

disp(' ‘'); 

dasa kpred % prediction number ‘) 
disp(’ '); 

disp@ @ar 

kpreds | = input(' _kpred = 1S’); 

eval([{ 'kpred = se ale 

ee 

disp 

disp(' ENTER OBSERVATION INTERVAL: '); 

disp@ mee); 

disp. dt % observation interval’) 
csp @ lee 

disp( 'THE TIME INTERVAL IS IN THE FORM: ') 

cispG al”) 

disp(' dt = time/60') 

disp. ). 

ena FOR A 10 MINUTE INTERVAL ENTER: dt = 10/60') 
dtsa— input( dtr = S72 

eval] dt = Sardis al) 


% TARGET SIMULATION PARAMETERS INPUT ROUTINE 


Cue 

disp (aur 

disper 

disp(' FOR THE PURPOSES OF SIMULATION,'); 


oli Stel ENTER THE TARGETS MOVEMENT PARAMETERS: ' ); 
eispl ); 


Gispe  ); 

disp(' [x % target initial x position (km)') 
disp(’ vx % target initial x velocity (km/hr)' ) 
disp(' ax % target acceleration in x (km/hra2)’ ) 
disp(' y % target initial y position (km)') 
disp( vy % target initial y velocity (km/hr)') 
disp(' ay % target acceleration in y (km/hr 2)" ) 
disp( fo]; % transmitter rest frequency (Hz)') 
Gasp: )s 

dasp( '): 

disp(' YOU MAY ENTER THIS IN THE FOLLOWING FORMAT: [x vx ax y vy ay fol): 
disp(' ); 

disp(' ‘): 

gasp Facdedete tevede ve te te tede ve este te kkdesedeak sek i seve ve a te ve Fete re ve ve dese Fete He sete Ke Fete Kee sek eK Fe ») 

disp(' * CAUTION: VELOCITY OF THE TARGET MUST NOT BE ZERO. * ') 

disp Sede sede seve de seve Te CK ee eT TKN Te TNE TEER I Te Be FETT EN ToT CI Te TC NN e) 

Egts = input( tgt - ye 

Seemce ter —  ,tests; ; |); 


tgt = reshape(tet, Pale 
fre Ctet (2,1) — 0 & tet(5,1) = 0) 


ele 

dusp() ):; 

displ ); 

disp(' FOR THE PURPOSES OF SIMULATION,’ ); 

disp(' ENTER THE TARGETS MOVEMENT PARAMETERS: ' ); 

eisp@. ); 

cise. ); 

disp(' ex *% target initial x position (km)') 
disp(' VX * target initial x velocity (km/hr) ) 
disp(' ax % target acceleration in x (km/hra2)') 
disp(' y % tarsetumultedal yeposition (km) ) 
disp(' Vy % target initial y velocity (km/hr)' ) 
disp( ay % target acceleration in y (km/hr,2)') 
disp(' eleva es % transmitter rest frequency (Hz)') 
disp(' ‘); 

disp(  ); 


disp( ' YOU MAY ENTER THIS IN THE FOLLOWING FORMAT: 
[x vx ax y vy ay fo] '' 


° t t 

disp( ); 

disp(" 23 

dis (ee Weslo se sloste ole Joule Jo cle Jo osteo close de oslo le Se clesle seve gle Je docle de stele ctecte cleclecte clonle dle cle le ecleclecledledleclesh levee i) 
disp(' * CAUTION: VELOCITY OF THE TARGET MUST NOT BE ZERO. * '‘) 
daisnG Peete Tete dee Fev HT TOR TE TEE TE I ENTE TTT TEE EUR TEE TE TERT ' + 
Sous — nput( tet.=  , s )% 

t 664 tf A 
eval([ tgt i ,tgts, ’ JQ 


tgt = reshape(tgt,7,1); 
end 


global xfol xfo2 spd kmax kpred dt tet; 
% OBSERVABLE MENU CHOICE ROUTINE 


choicel = -10; 
while choicel = 0 


ele 


daspiGuwae 
eispe a): 
clotsyel(! 2 
dispGeac lelaine): 
disp(titlelinel); 
disp): 
disp 
disse 
dasa e ENTER THE METHOD OF OBSERVATION USED FOR THIS RUN: '); 
dispGune), 
disp(' 1 Bearing/Bearing’ ne 
disp( 2. Bearing/Range’ ); 
disp(' 3. Range/Range ); 
disp(' 4. Shifted Doppler Frequency’ ye 
disp( 5. Display the last graph’); 
daisp(s 6. Change the number of observations” ye 
disp(' 7. Change the number of predictions | ys 
Gasp 8. Change the observation interval’ )5 
disp 9. Change position of first observer’ ); 
disp(' 10. Change position of second observer ye 
disp(' 11. Change observer movement speed’); 
daspG 12. Change target simulation parameters "); 
disp(' °); 
disp Oe Chose Ne 
dispG@a on 
choicel = input(' ENTER YOUR CHOICE: '); 
if choicel == 
bugbmg. 
elseif choicel == 2 
Droune. 
elseif choicel == 3 
pa ebca mena 
elseif choicel == 4. 
doppler; 
elseif choicel == 5 
shg 
pause 
elseif choicel == 6 
cle 
disp C aon 
dise. ENTER DESIRED NUMBER OF OBSERVATIONS: '); 
eelsyel( |) 
disp( kmax % observation number ') 
dispG aaa 
dispGa 


disp(' PLEASE MAKE THIS GREATER THAN 5 SO THAT THE KALMAN GAINS’ ) 


disp('CAN STABILIZE. ') 


kmaxs = input( 'kmax = Oo, Se 
eval({'kmax = ',kmaxs,';']); 
elseif choicel == 
Elec 
dispG@ sae 
disp('ENTER DESIRED NUMBER OF PREDICTIONS: '); 
disp( a: 
disp(' kpred % prediction number ') 
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ai15piG 0). 


eispGas). 
kpreds | = input(' Kpred = Dsiae 
eval({ 'kpred = [ne aie 
elseif choicel == 8 
ere 
disp). 
disp(' ENTER OBSERVATION INTERVAL: '); 
Gaisp(  ) 
disp(' dt % observation interval') 
disp(' ‘); 
disp('THE TIME INTERVAL IS IN THE FORM: ') 
ais a) 
disp(' dt = time/60' ) 
disp sar 
disp('FOR A 10 MINUTE INTERVAL ENTER: dt = 10/60') 
gasp 9.) 
ales input (' dt = ‘,'s'); 
eual(| dt = dts, ; |): 
elseif choicel == 9 
ele 
disp(' '); 
disp( ae 
disp(' 33 
disp( 
disp(' ENTER THE FIRST OBSERVERS INITIAL PARAMETERS: ' ); 
disp(' |); 
disp(' '); 
disp(' mrol = | x % observer x position (km) ') 
disp(' VX ferellocity x. darection (en/nr)  ) 
disp(' y % observer y direction (km)') 
disp Se le pevellocityveyecitection (km/hr) ) 
disp(' '); 
disp('YOU MAY ENTER THIS IN THE FOLLOWING FORMAT:[x vx y vy]''); 
Seolse— input( xfol =’ ,'s'); 


eealq| xfol = ,xfols, '; }); 
xfol = reshape(xfol,4,1); 


elseif choicel == 10 
ele 
dispe |); 
disp( |); 
Gispo . )- 
dis p(' "y; 
dispt ENTER THE SECOND OBSERVERS INITIAL PARAMETERS: '); 
Gasp 5 
ais (1) 
disp(' xfo2 =[ x % observer x position (km)') 
disp(' VX % velocity x direction (km/hr) ) 
disp( y % observer y direction (km)') 
ol oll | vy |: % velocity y direction (km/hr)') 
disp(' 


)5 
disp(’ YOU MAY ENTER THIS IN THE FOLLOWING FORMAT: [x vx y Vales 
EOZS = input ( xfo2 = _ Se 
eval({ ' xfoZz = LOLS 5... ; alii: 
xfo2 = reshape(xfo2,4,1); 
elseif choicel = 11 
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ealte 
alsigro 
disp(' ENTER OBSERVER MOVEMENT SPEED: '); 
dis Game): 
disp(' spd % observer speed in km/hr ') 
disp(' er 
dispc *); 
spds = input (| spd = Se 
eval(| spd = Msmedsmuemul) : 

elseif choicel = 12 
Circ 
ellie): 
disoG@e J: 
disp(' FOR THE PURPOSES OF SIMULATION,'); 
disp(' ENTER THE TARGETS MOVEMENT PARAMETERS: '); 
disp(' 
daspe: 
disp ee % target initial x position (km)') 
disp Vx % target initial x velocity (km/hr) ' ) 
Aspe ax % target acceleration in x (km/hra2)’ ) 
dds pt. y % target initial y position (km)') 
disp(' Vy 7 target initialey velocity (km/hr) ') 
disp(' ay 7 target acceleration in y (km/hra2)' ) 
disp Goete % transmitter rest frequency (Hz)') 
elise, | 
disper 
re YOUSMAY ENTER aie IN THE FOLLOWING FORMAT: 
[Ve ove coun 


t 


dis p(s oe 
dige ane 
disp( lo vedivetidedsedidivsstisssedesiteotstecoinionistvinicdededededetetiotckicttelekiectctoteien 1 ) 
disp(' we eee VELOCITY we THE TARGET Rae ane BE ae * T+) 
disp(' bare e aa Sa ehtlinc couric niichirhecy ia Giri sdricbraecise rachis ! ) 
tgts = input(' 'tgt = ie 
Seu (eesee S 9 iets ye |e 
tgt = reshamet eet” ioe ee 
if (tgt(2,1) == 0 & tgt(5,1) == 0) 
Ele 
digi 
dist): 
disp(' FOR THE PURPOSES OF SIMULATION,'); 
disp(' ENTER THE TARGETS MOVEMENT PARAMETERS: ' ); 
disp(' '); 
dis cea): 
disp(' libaex % target initial x position (km)') 
dispe vx % target initial x velocity (km/hr) ') 
Garcia ax % target acceleration in x (km/hra2)') 
dispe y % target initial y position (km)') 
disp(' vy % target initial y velocity (km/hr) ') 
elalcyay ay % target acceleration in y (km/hra2)') 
disp( fo]; % transmitter rest frequency (Hz)') 
disp(' '); 
digo 


disp(' YOU MAY ENTER THIS IN THE FOLLOWING FORMAT: 
[x vx ax y vy ay £ole aoe 
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« ! { 
oto aaa | 
di ( ! P 

isp . 
disp(' seveseccacdvdeavseckesesksccecccek vcs scacscteededctektekKicdseddeiek dedteckdedk ttc tt Fe ) 


disp(' * CAUTION: VELOCITY OF THE TARGET MUST NOT BE ZERO. *") 
disp(' dere tede rede dereaede dee sede ere ye Nese ee A KITE HT HA ICT HTH NIT HE HRN KI TE | + 


tets = input('tegt = ','s'); 
evap tet —smtces sas. | )s 
Poem ereshapecece... 1); 
end 
end; 
end; 


% JOHN A. HUCKS II 
7 FILE NAME : BRGBRG.M 
vo EXTENDED KALMAN FILTER PROGRAM USING BEARING/BEARING MEASUREMENTS 


7o THIS PROGRAM SIMULATES THE TRACK GENERATED BY AN EXTENDED KALMAN 


7% FILTER, WITH INPUTS FROM A FUSED SENSOR ARRAY AS IT TRACKS A TARGET 
vo ACROSS AN X-Y GRID SPACE (PLRS FLAT EARTH PROJECTION). 


clg 

clear eex eey xto 

!'erase brgbrg. met 

diary bearing 

7 OLTATE EQUATIONS 

°° OBSERVATION TIME INTERVAL (input from marine. m) 


7% OITATE TRANSITION MATRIX 


phi=[1dto0 0 
Om 1G" 50 
OPE sie t 
On 50 0" taalle 


eee ele) NOISE COEFFICIENT MATRIX 
del =[{ dt*dt/2 0 


dt 0 
0 dt-dat/Z 
0 dt le 


fone VARIABLES, FILTER ESTIMATES, TIME, AND OBSERVED DATA 
% NUMBER OF OBSERVATIONS (input from marine.m) 


% EorIMATED TARGET STATE 
xte = zeros(4,kmax); % initial est. target state 


% OBSERVER (SENSOR) INITIAL POSITIONS AND VELOCITIES 
% (input from marine.m) 


% OBSERVER SPEED (input from marine. m) 


*% ILNITIAL TIME SETTING 
Time = zeros(1,kmax),; 


% ACTUAL TRACK POSITIONS (tgt vector input from marine. m) 


xX = zeros(1,kmax); 
y = zeros(l1,kmax); 
T = 0; 
for k= 1 
x(k) = totl le: 
y(k) = tgt(4,1); 
end 
for k = 2: kmax 
T = T+dt; 
x(k) = (0. S*tet(35 2) (1A2) 2 (eect? ee cot eles 
y(k) = (0. S¥tgt(6,1)*(Ta2))+(tgt(5, 1). *T)+(tgt(4,1)); 
end 
zpos =[ x 
wie 


% INPUT OBSERVATION VALUES (BEARING/ BEARING) 
% INITIAL OBSERVED TARGET STATE 


xtoC@l) = | terCie® % (km in x direction) 
fe 2) % (km/hr in x direction) 
tgt(4,1) % (km in y direction) 
pet (aaa il. % (km/hr in y direction) 


% SIMULATIONS COUNTER OOF 
xfoa = zeros(4,kmax); 
xfoal: 4) =sreniG:- 1 )- 
xfol “= s0rea: 
xfob = zeros(4,kmax); 
xfob(: [I= sxtol7. 1): 
xfo2 = xToe: 
k = 0; 
for kl=1: 10-4max 
k= ko ae % actual loop counter 
if (k == kmax) % check for termination 
break 
end 


% UPDATE FORWARD OBSERVER'S/ FAC'S POSITION (IF THEY ARE MOVING) 


if k <= 3 
way = pi/2; 
else 
way = -(3*pi/4); 
end 
if spd = 0 
xfol(ivicl) = xfoluiek):. 
xfol@oereije— xicl(3.k)- 
Xxfo2(1,k#1) = xfo2(1,k); 
xfo2Goekt]) = xioz@rk). 
else 
xfol Gia xfo1(1,k) + spd*sin(way)*dt; 


amen! .)kearil)) 
xicl(erra) 
xfol(4 ee) 


spd*sin(way); 
xfol(3,k) + spd*cos(way)*dt; 
spd*cos(way); 


nou ue ut 
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mre? ( 1 kr) 
hnO2( 2,k+1) 
SEOZ(3.KT1) 
xfo2(4,k+1) 
end 
end 


MEoZiolek) e+ spd*sin(way )*dt; 
spd*sin(way); 
xfo2(3,k) + spd*cos(way)*dt; 
spd*cos(way); 


Z2EOSiGl.: )eemnol( 1. : ): 

ZpOs.2,: ) sexrol(3,: ): 

Zoos) ear XtOZt 1s): 

ZOOS2,°” ) =2XtOZG3,.° )s 

brgl = atan2(deltlx,deltly); % bearing thtal 
= atan2(delt2x,delt2y); % bearing thta2 

Zeps = | breil; breg2}; 
= zeros(4,kmax); 

meolcl,: ) = zpos(1,: ); 

heeol(3,:) = zpos(2,: ); 

mio 1) = xto(: ,1); 

MEO — Xtol; 


Ou 
@ 
4 
ct 
NO 
rad 
1 


70 OLTATE EXCITATION AND MEASUREMENT ERROR COVARIANCE MATRICES 


weary = 0.000); % Variance of linear acceleration 
varth = 0.01096; % variance of angular acceleration 
R =[ 0.000005 0 

0 ©. 000005 |; % measurement noise 


% INPUT OBSERVATION VALUES WITH NOISE ADDED 
rand( ‘normal’ ) 
Zeps — Zebs + sqrt(R(1,1))*rand(zobs); 


% INITIAL KALMAN MATRICES AND OBSERVATION 
I = eve(4); 


70 INITIAL KALMAN P MATRIX 

PO =[{ 800 0 0 0 “7 reset value of Error Covariance 
0 800 0 0 
0 0 800 0O 
0 00 S00]: 


%  REINITIALIZING P MATRIX 


P= PO: 

PA LMNAN ESTIMATE 

mreq>,1) = [ xto(1,1) ckKMmeoex, direction ) 
beote2 % (km/hr) 
MO Ls) % (km - y direction) 
E2ECs..) 1: % (km/hr) 

Bored = xte(: , 1); 

% SIMULATION OF KALMAN FILTER OPERATION 

% NUMBER OF BAD OBSERVATIONS 

nbad = 0; Aetniemal setting bad obs. centr. 

% SIMULATION COUNTER LOOP 

k = 0; 


fOr fee lo-cemvas 
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%: 


k= k +. 1; % actual loop counter 

if (k == kmax) % check for termination 
break 

end 


UPDATE FORWARD OBSERVER'S/ FAC'S POSITION (IF THEY ARE MOVING) 
Lik <=<35 


way = pi/2; 
else 
way = =(3%pi/4), 
end ‘ 
if spd = 0 
xfol@ieik+l) = xferer kk); 
xfel(3S,k+1) = xfol@sak):. 
xfoz(1,kt1) = xfeZele). 
xf£62(3,kt1) = x8eZ@3-)- 
else 
xfol(1,k+1l) = xfol(1,k) + spd*sin(way)*dt; 
xfol(2,k+1) = spd*sin(way); 
xfol(3,k+1) = xfol(3,k) + spd*cos{ way ade. 
xfol(4,k+1) = spd*cos(way); 
xfoZ( 1, kel) = xfeo2( Take spd samt way sat: 
xfo2(2,k+1) = spd*sin(way); 
xfo2(35k+1) = xfo2z(3,k)Meespd*cos( way ade. 
xfo2(4,k+l) = spd*cos(way); 
end 


% PROJECT AHEAD KALMAN STATE ESTIMATE 
xte(: ,kbl)e=sohe tee). 


% UPDATE Q 
vt = sqrt((xte(2,kt1))a2 + (xte(4,k+1))a2); 


gqll = vary * (xte(Z,ktl1) / vt)a2 + varth* (xte@oei 7 
q22 = varv * (xte(4,kt1l) / vt)a2 + vVarth “se ip 
qlZ2 = xte(Z,kt1) * xte(4ikt]) * Clvarv/vt)az = varth =). 
q21 ="qiZ- 


O1 =") aerate 
qc ime 2 |; 
Q = del*Q1*(del ); 


% PROJECT ERROR COVARIANCE 
P = phi*P**(phi') + Q; 


% UUPDATE SH 


deltlx = xte(1,kt+1l) - xfol(1,k+1); 
deltly = xte(s2k-1) = xfol@rie1)- 
delt2x = xteQ@i.ktl]) = xfozGir+1)- 
delt2y = xte(3,kt1)) = xfoz@jsrcel): 


r12 = (deltlx) 2 + (deltly) 2; 

r22 = (delezx je (delt2 rn 2. 

rl SQreGri2). 

1 SCreCr22Z- 

H=[ delely =l250 -del etx) m2 aw 
delt2y7 222.00 =-delt2x/ r22 sa0ae 


% GET NEW MEASURED ESTIMATE 


Sigete— atanc(deltix,deltily); % bearing estimate thtal 
zhat2 = atan2(delt2x,delt2y); % bearing estimate thta2 
eaar = | zhati 

Zia ene: 


% COMPUTATION OF KALMAN GAIN 
vresid = (H*P*(H') + R); 
G = P*(H') / vresid; 


% variance of the residual 
% Kalman gain 


% COMPUTATION OF UPDATED ERROR COVARIANCE 
P = (I - G*H)*P; 


70 COMPUTATION OF RESIDUAL 
resid = zobs(: ,kt+1) - zhat; 


% IF RESIDUAL IS TOO BIG, THEN RESET P AND DO CALCULATIONS AGAIN. 
fmem@dbs( resid’ 1)) >= 1, 0*sqrtCabs(vresid(1,1))) ... 
fmabs@cesid(2)) > 1, 0*sqrt(abs(vresid(2,2))) ... 
& nbad == 0) 
oa 
k-1; 
PO; % reset P 
else 
w COMPUTATION OF UPDATED ESTIMATE 
xte(: ,k+t1) = xte(: ,k+1) + G*resid; 


% ERROR ELLIPSOID PLOTTING ROUTINE 
M eee 1) ixteC3,kt1) [ite 
ee Ci) 

ACS 59) Rh GSES ea 
esp), — ezzelliptl,kK,.6€0,50,0); 
eex(: ,k) xp: 
eey(: ,k) = yp; 


Hoi 


PREDICTED (FUTURE TRACK) USING KALMAN ESTIMATE 
(number of predictions desired input from marine. m) 
meee: 51) = xte(:.,k+1); 
acc = zeros(2,1); 
for 1 = 1: kpred 
if k >= 3 
BGex = (| xreuc kt) jexte( 2.) )/dt; 
accy (xte(4,k+1)-xte(4,k))/dt; 
accu= | .accx=accy)|: 
if accx >= § 
break 
acc = zeros(2,1); 
elseif accy >= 5 
break 
acc = zeros(2,1); 
end 
end 
tpred(: ,1+1) = phi*tpred(: ,1)+del*acc; 


3? 3s? 


% Kpred is # of predictions 


end 
nbad = QO; 
end 
% PLOT OBSERVED AND ESTIMATED TRACKS (TARGET AND OBSERVER) 
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ahaha: 1: k+1),xfol(3,1:k+1),'xw' ,xto(1,1: k+1),xto(3,1: k+1) 
ole 


xte(1,1:k+1),xte(3,1:k+1),' +w' ,xfo2(1,1:k+1) ,xfo2(3,1:k+1),'xw',... 
ZOOS 1,4: k+1), ZpDOS (2a ke OW ,tpred(1,: WS tpred(3,: es eo! veo 

eex jeey, ‘=r i etad 

title(’ ACTUAL/OBSERVED/ESTIMATED/PREDICTED TGT TRKS USING BRG/BRG ') 


xlabel(' DISTANCE X'),ylabel('DISTANCE Y') 
end ° 
meta brgbrg 


% SOUNGAe HUCKS 12 
® ELLE PGE] RNGRNG a 
% EXTENDED KALMAN FILTER PROGRAM USING RANGE/RANGE MEASUREMENTS 


% THIS PROGRAM SIMULATES THE TRACK GENERATED BY AN EXTENDED KALMAN 
% FILTER, WITH INPUTS FROM A FUSED SENSOR ARRAY AS IT TRACKS A TARGET 
% ACROSS AN X-Y GRID SPACE (PLRS FLAT EARTH PROJECTION): 


clg 

clear eex eey xto 
lerase rngrng. met 
diary rngrng 


% OTATE EOQULIMG Ss 
% OBSERVATION TIME INTERVAL (input from marine. m) 


% STATE TRANSIGIOs MATRIX 
phi = | -ledrae ee 


> i a My a | 

OOoOr- 

Or © 
ee 
ct 


ale 


% SYSTEM NOISE COEFFICIENT MATRIX 
del =[ dt*dt/2 0 


dt 0 
0 Ged 2 
0 dt lie 


*% SIATE VARIABLES, FILTER ESTIMATES, TIMED SAND OBSERY Eur AlA 
% NUMBER OF OBSERVATIONS (input from marine. m) 


% ESTIMATED TARGET STATE 
xte = zeros(4,kmax); % initial est. target state 


% OBSERVER (SENSOR) INITIAL POSITIONS AND VELOCITIES 
% (input from marine.m) 


% OBSERVER SPEED (input from marine. m) 


eT L AL, TIME SETLING 
Time = zeros(1,kmax); 


% ACTUAL TRACK POSITIONS (tgt vector input from marine. m) 


xX = zeros(1,kmax); 
y = zeros(1,kmax); 
i= 50; 
for k= 1 
x(k) = tgt(1,1); 
y(k) = tgt(4,1); 
end 
for Kk = 2: kmax 
T = T+dt; 
Mae Os om tCoOtC Sede Pee + tet 2, 1)24T)+(tetC1,1)9; 
y(k) = (0. 5*tgt(6,1)*(Ta2))+(tgt(5,1). *T)+(tgt(4,1)); 
end 
zpos = [ x 


yo 


7° INPUT OBSERVATION VALUES (CRANGE/RANGE ) 
® OBSERVED TARGET STATE 

xfoa = zeros(4,kmax); 

Beeoaw: 1) = xfol(: ,1); 

xfol = xfoa; 

xfob = zeros(4,kmax); 

moo: .1) = xfo2(:,1): 


xio2,— xfob; 
mec, 1) = | tet(1,1) i (kinediex direction) 
eyecare) UK, heen x direction) 
tgt(4,1) moCKkM Inv da section) 
feces.) ) | 3 % (km/hr in y direction) 
%o SIMULATION COUNTER LOOP 
k = QO; 
for k1=1: 10*kmax 
hoa ob 1; % actual loop counter 
if (k == kmax) % check for termination 
break 
end 


*% UPDATE FORWARD OBSERVER'S/ FAC'S POSITION (IF THEY ARE MOVING) 
if k <= 3 


way = pi/2; 
else 
way = -(3*pi/4); 
end 
if spd = 0 
Moule) = xfol(1,k); 
Snomes kt) ) = xfol{3,k); 
Stove) = xto2( lak): 
Kto2(@sekrl = xfo2(3,k); 
else 
xfol(1,k+1) = xfol(1,k) + spd*sin(way)*10*dt; 
xfol(2,k+t1) = spd*sin(way); 
xfol(3,k+1) = xfol(3,k) + spd*cos(way)*10*dt; 
xfol(4,k+1) = spd*cos(way); 
xfo2(1,k+1) = xfo2(1,k) + spd*sin(way)*10*dt; 


xfo2(2,k+1) = spd*sin(way); 
xfo2(3,k+1) = xfo2(3,k) + spd*cos(way)*10*dt; 
xf02(4,k+1) = spd*cos(way); 

end 


end 

rl = sqrt((zpos(1,: )-xfol(l,: )). .2 + (2pos(2Z,? )-xtel(g uae ee 
r2.= sqrt((zpos(1,: )-xfozCly: a2 + (zpos(2,: )-xfioZz( oy een 
zobs=[1r1; r2]; 

xtol = zeros(4,kmax); 

Xeon Gl je = 2pos( 1 7. oe 

Xtoils.:.) ZpOS( 2455 

XCOd aie) MUCl. 10), 

Xto =cxtel; 


% STATE EXCITATION AND MEASUREMENT ERROR COVARIANCE MATRICES 


varv = 5.0; *% variance of linear acceleration 
varth = 1.0; % variance of angular acceleration 
R =[ 0.005 0 

OmeOnO05 ir % measurement noise 


% INPUT OBSERVATION VALUES WITH NOISE ADDED 
rand( ‘normal’ ); 
Zens = Zo0bs 4 ssaqrt( h(i, 1))-rand( zeus 


% INITIAL KALMAN MATRICES AND OBSERVATION 


I = eye(4); 
% INITIAL KALMAN P MATRIX 
PO = [9250 0m Gin % reset value of Error Covariance 


We 250 0 
0 Oazsoo 
eo 6 ASO) 

® REINITIALIZING P MATRIX 


Poe 

% KALMAN ESTIMATE 

xte(:,1) = (iixccG 1) % (km - x direction) 
ote |) yo Cem ye) 
XEOCS 6 L) % (km - y direction) 
14a (5) 5 ae % (km/hr) 


tpred = xtet.4)), 


7% SIMULATION OF KALMAN FILTER OPERATION 
vo NUMBER OF BAD OBSERVATIONS 


nbad = 0; % initial setting bad obs. cntr. 
% SIMULATION COUNTER LOOP 
k = 0; 
for k1l=1: 10*kmax 

k= ka % actual loop counter 

if (k == kmax) % check for termination 

break 
end 


% UPDATE FORWARD OBSERVER'S/ FAC'S POSITION (IF THEY ARE MOVING) 
if k <= 3 
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mid = Biel) 2; 


else 
aye a Oo pl) 4) 

end 

if spd = 0 
mrolt il ,k+l) = xfol(1,k);: 
mrol(i 3 k+l) = xfol(3,k): 
Shoe ()kt)) = xfo2(1.k): 
pe O2 (oro) = xfo2(3.,k); 

else : 
xfol(1,kt1) = xfol(1,k) + spd*sin(way)*10*dt; 
xfol(2,k+1) = spd*sin(way); 
ReQlesukn)) — xtOl(3,k) + spd*cos( way )* 10*dt; 
xfo1l(4,k+1) = spd*cos(way); 
xfo2(1,k+1) = xfo2(1,k) + spd*sin(way)*10*dt; 
xfo2(2,k+1) = spd*sin(way); 
xfo2(3,k+1) = xfo2(3,k) + spd*cos(way)*10*dt; 
xfo2(4,k+1) = spd*cos(way); 

end 


7 PROJECT AHEAD KALMAN STATE ESTIMATE 
meet: ,Ktl) = phi*xte(: ,k); 


*% UPDATE Q 
vt = sqrt((xte(2,kt+1)) 2 + (xte(4,k+1)) 2); 


eee = Vary * (xte(2,k+1) / vt) 2 + varth * (xte(4,k+1)) 2; 
Pe — Vary * (xte(o kt!) / vt) 2 tevarth * (xte(2,k+1)) 2; 
gue = xce(2Z,kt]l) = xte(4,k41) * ((varv/vt) 2 = varth ); 
qzi = gql2; 
Ql =f qll ql2 

ee e229 |; 


Q = del**Q1*(del'): 


% PROJECT ERROR COVARIANCE 
P = phi*P**(phi') + Q; 


FUE DALE H 

aemeix = xteCi,k+l1) = xfol(1,k+1); 

gettly = xteC3,kt1) = xfol(3,k+1); 

delegs = xte(l]1,k+1) = xfo2(1,kr1); 

Geieey — xte(3 k+l) - xfo2(3,k+1); 

mie — (deltix) 2 + (deltly) 2: 

meee (aelt2x) 2 + (delt2y). 2: 

mie Sqntcr )2); 

mee— Sqmt(r22); 

Pe aieedeltixjrl 0 . deltly/rl .0 
dellezx7r2 0 delt2y/r2 0}; 


*% GET NEW MEASURED ESTIMATE 


Ze) sNe A es gall 
feo. 
% COMPUTATION OF KALMAN GAIN 
vresid = (H*P*(H') + R); % variance of the residual 
G = P*(H') / vresid; % Kalman gain 
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% COMPUTATION OF UPDATED ERROR COVARIANCE 
P = (I - G*H)*P; 


% COMPUTATION OF RESIDUAL 
resid = zobs(: ,K+l jmeeeznac, 


% IF RESIDUAL IS TOO BIG, THEN RESET P AND DO CALCULATIONS AGAIN. 
if Cabstresucde))) 1. i. O%*sqrt(abs(vresid(1, a Ya See 

} abs(resid(2)) > 1. O*sqrt(abs(vresid(2, 2))) cee 

& nbad = 0) 


Oi: % reset P 


% COMPUTATION OF UPDATED ESTIMATE 
xte(: ,k+1) = xte(: ,k+1) + G*resid; 


% oar ELLIPSOLD Lei ING ROUTINE 
= [xte(1,k+1) xte(3,k+1)] | 
rol Pie tye Ci eop) 
P(3,1) P(3,3) J; 
[xp vp) eawerrel pg - 60), 25.000 
eex(: ,k) er 
ee, (7 5k) eae, 


% PREDICTED (FUTURE TRACK) USING KALMAN ESTIMATE 
% (number of predictions desired input from marine. m) 
tpred@: m= exe. kta 
AGG = —Zemos( 2 19. 
for 1 = 1:kpred % kpred is # Gi predictions 
if k >= 5 
acexe= (xte( 2 kr exteCZn)) dat 
accy = (xte(4,k+1)-xte(4,k))/dt; 
ace = aacex; accy |< 
if wacex = 5 
break 
ace = 0:0]; 
elseif accy >= 5 
break 
AC Ce 80500]: 
end 
end 
tpred(: ,1+1) = phi*tpred(: ,1)+del*acc; 
end 
nbad = OQ; 
end 
% PLOT OBSERVED AND ESTIMATED TRACKS (TARGET AND OBSERVER) 
plot( xfoleiea: K+1),xfol(3, 1: k+1), xw ,xteql))k1r 
xte(3,1:k+1),'tw',... 
EO 1: k+1),xfo02(3,1:k+1), xw  ,zpos(1,1:k+1),zpos(2,1:k+1), ow ,... 
xto(1,1:k+1),xto(3,1:k+1),'.b',tpred(1,: ),tpred(3,:),'*g',... 
GX, CCV ee end 
title( 'ACTUAL/OBSERVED/ESTIMATED/PREDICTED TGT TRKS 
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USING RNG/RNG OBS'),... 

xlabel( 'DISTANCE X'),ylabel('DISTANCE Y') 
end 
meta rngrng 
itary ort 


JOHN A. HUCKS II 
FILE NAME : BRGRNG.M 
EXTENDED KALMAN FILTER USING BEARING/RANGE MEASUREMENTS 


THIS PROGRAM SIMULATES THE TRACK GENERATED BY AN EXTENDED KALMAN 
FILTER WITH INPUTS FROM A FUSED SENSOR ARRAY, AS IT TRACKS A 
POTENTIAL TARGET ACROSS AN X-Y GRID SPACE (PLRS FLAT EARTH 
BROJECTION). 


SLSL SL SL Fs®slQ ae 


clg 

clear eex eey xto 
!erase brgrng. met 
diary brgrng 


fae SLATE EQUATIONS 
% OBSERVATION TIME INTERVAL (input from marine. m) 


* STATE TRANSISTION MATRIX 
phi=[1dt0o 0 


ie 0 0 
Gro. dt 
eo 0 1 |; 
fe oYSOTEM NOISE COEFFICIENT MATRIX 
del = [ dt*dt/2 0 
dt 0 
0 dt*dt/2 
0 dt es 


% STATE VARIABLES, FILTER ESTIMATES, TIME, AND OBSERVED DATA 

% NUMBER OF OBSERVATIONS (input from marine.m) 

ee ESLTIMAITED TARGET STATE 

xte = zeros(4,kmax); % estimated target state 

% OBSERVER (SENSOR) INITIAL POSITIIONS AND VELOCITIES (input from marine. m) 
% OBSERVER SPEED (input from marine. m) 


Poa NOT AL TINE SETUP 
Time = zeros(1,kmax); 


% ACTUAL TRACK POSITIONS (tgt vector input from marine. m) 
x = zeros(1,kmax); 
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y = zeros(1,kmax); 


Ap 0.2 
forake=— 1 
| x(k) = tgt(1,1); 
y(k) = tgt(4,1); 
end 
for k = 2: kmax 
T= T+dt; 
x(k) = (0. 5*tgt(3,1)*(Ta2) )H(tetl 2, 21) (cot ee 
y(k) = (0. 5*tgt 6, 1)*(Ta2Z) )4CCet Co, PAT) tte, 
end 
zpos =[ x 
y |; 


% INPUT OBSERVATION VALUES (BEARING/RANGE): 
xfoa = zeros(4,kmax); 

xfoa(: , 1) = fol). 

Rio xia 

xfob Zeros oranda) 

xftobG@, 1) = xfoz@ awe 


xfo2 =extfob: 

xfor = zeros(4,kmax); 
zobr = zeros(1: kmax); 
rngl = zeros(1: kmax); 
rng2 = zeros(1: kmax); 


deltlx = zeros(1: kmax); 


delt2x = zeros(1: kmax); 
deltly = zeros(1: kmax),; 
delt2y = zeros(1: kmax); 


thtal = zeros(1: kmax); 
thta2 = zeros(1:kmax); 
for k = 1: kmax 
rgiCk)>— saree zpos hyo xtol( lim) 
CCzpos( 2,k)=xtolCo. ka. Zee 


esate 


rng2(k) ="sart(G zpos ( ak) =x8o 20 ee. 2 eee 


C(2p0s2) kh jiesetecg kina 
deltlxG@e= pes 1 xe. 


deltlyCk) = Zeese@2, k)-xfol(3 sky: 
delt2x(k) = zpos(1,k)-xfo2(1,k); 
delt2y(k) = 29552) -xfo7G] kb): 
thtal(k) atan2(deltix(k),deltly(k)); 


thta2(k) = atan2(delt2x(k),delt2y(k)); 
end 
zobr = [ thtaleraelerntaZz. ciel 


% MEASUREMENT REFERENCE CONVERSION ROUTINE: 


fOr ech eeaa 
Xt =stccel 1); 
xtZ =Gecre 1). 
VYtl="terea a): 
yt2 = Egt(s, 1); 
Xtr =GGxet ce) 2; 


VCE = (ytliye2 )y 2: 
deixr:=<xtr@i cnet). 
delyr = vtrc1, ent }; 
thtar(l,cnt) = atan2(delxr,delyr); 
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micr( 1,ent j= sqrec(delxr 2)+(detyrn2)); 


end 

foOreent = 2: kmax 
Mrl(ilvent) —=Z7Op72cnt )*Sin( Zobr( b. cne) ); 
Mel lent ju=—— 2Oba2. cnt )*cos( zobr( 1,cnt) ); 
xe l cnt) = zobr(4,ent )*sin( zobr(3,cnt)); 
met ),cnt) = zobr(4,cent )*cos( zobr(3,cnt)); 
Xer(lacmepe— (xt tt Locnt )+xt2(1,¢cnt))/2; 
Vane cnty— eC vimmel ems tryt2( 1,cnt) )/2; 
delxr = xtr(l,cnt); 
delyr = ytr(l1,cnt); 
titar( 1 ,cnt) = .atan2(delxr,delyr;, 
rngr(l,cnt) = sqrt((delxr 2)+(delyra2)); 

end 


% CONVERTED OBSERVATION MATRIX: (CONVERSION OBSERVER TAKEN AS THE ORIGIN) 
*% OBSERVED TARGET STATE 


Meote il) = | tet(1,1) meckmein x direct ion) 
bot (251) oS Cem /nrein x direction) 
ie Gare lk) % (km in y direction) 
fOr te Sen) % (km/hr in y direction) 

geos — [| thtar 

ther); 

xtol = zeros(4,kmax); 

maeolt ly: ) = zpos(1,: ); 

meer(s,:) = zpos(2,: ); 

paren. 1) = xto(: ,1); 


MEO — Xtol; 


°° OTATE EXCITATION AND MEASUREMENT ERROR COVARIANCE MATRICES 


weary = 5.0; % variance of linear acceleration 
varth = 1.0; % variance of angular acceleration 
R=[ 0.0005 0 

0 Oe 00 5015 % measurement noise 


fo LNPUT OBSERVATION VALUES WITH NOISE ADDED 
rand('normal'’ ) 
noise = sqrt(R(1,1))*rand(zobs); 


% INITIAL KALMAN MATRICES AND OBSERVATIONS 


I = eye(4); 
%o INITIAL KALMAN P MATRIX 
Po—[{ 700 0 0 O % reset value of error covariance 


0 700 0 0 
0 0 700 O 
0 0 0 700 }; 


% REINITIALIZING P MATRIX 
P = PO; 


% INITIAL KALMAN ESTIMATE 
meee, 1) = | xto( 1,1) 
weec2, 1) 
xeEo( 3S, 1) 
epael Gee alle 
Epred = xte(: mb): 


7} 


% SIMULATION OF FUSED SENSOR OPERATION USING EXTENDED KALMAN FILTER 
% NUMBER OF BAD OBSERVATIONS 


nbad = Q; *% initial setting bad obs. cntr. 
k = 0; 
for k1=1: 50*kmax 
k=k +1; *% actual loop counter 
if (k == kmax) % check for termination 
break 
end 


% UPDATE OBSERVER'S/FAC'S POSITION (IF THEY ARE MOVING) 
if k <= 3 


way = pi/2; 
else 
way = ~-pi/4; 
end 
if spd = 0 
xfol(] kt le ="xtolG a. 
xfol(3ykt ir ="xroleenk). 
xfoZC1 kt] )S=6xro2ZClk ):. 
xfo2(3j kt!) —exfoc( ce): 
else 
xfol(1,k+1) xfol(1,k) + spd*sin(way)*dt; 
Xf£ONG2 ke) spd*sin(way); 


XEOLGS kao) 
xftol(4.k+1) 


xfo1(3,k) + spd*cos(way)*dt; 
spd*cos(way); 


x£o2 Cl kas) REOZCI jk) t Spd*sin(war jade, 
xfo2( 2k spd*sin(way); 
xfo2(3 7 ks) xfo2(3,k) + spd*cos(way)*dt; 
xEO2( 4k) spd*cos(way); 

end 


% PROJECT AHEAD KALMAN STATE ESTIMATE 
xte(: ,k+tl1) = pha*xte(: ck): 


% UPDATE Q 
vt = sqrt((xte(2,k+1))a2 + (xte(4,k+1))a2); 


qil = varv * (xte(2,k+1) / veda2oteverth Gx tees ke 
q22 = varv * (xte(4,k+1) / vt)a2 + varth * (xte(2,k+1))a2; 
qli2 = xte(2,k+1) * xte(4,k+1) * (Cvarv/vt)a2 - varth ); 
q21 = ql2; 
Ql = [ qlivaiZ 

2 lamee 1; 


Q = del*Q1*(del ); 


% PROJECT ERROR COVARIANCE 
P = phi*P*(phi') + Q; 


% UPDATE H 

deltx = xte(1,k+tl1); 

delty = xte(3,k+t1); 

r2 = (deltxmeeecdelty 2: 

r Ssqrt@ 2), 

H=[ delty/r2 0 -deltx/r2— 0 
de lex) rae deltiyv/i= w0min 


% GET NEW MEASURED ESTIMATE 
delx = xte(1,k+1); 

dely = xte(3,k+1); 

zhatl = atan2(delx,dely); 


zhat2 SQGt deixnc teaelvn2); 
zhat = [( zhatl 
ZniatZ | ; 
% COMPUTATION OF KALMAN GAIN 
vresid = (H*P*(H') + R); % variance of the residual 
G = P*(H') / vresid; % Kalman gain 


% COMPUTATION OF PROJECTED ERROR COVARIANCE 
Beet GHP; 


% COMPUTATION OF RESIDUAL 
resid = zobs(: ,k+l) - zhat; 


% IF RESIDUAL TOO BIG, RESET P AND DO CALCULATIONS AGAIN. 
Memdes«resid( l)) = 1.0*sqrtCabs(vresid(1,1))) «.. 
iabs( resid( 2) je 1.0*sqrtCabs(vresid(2,2))) ... 
& nbad < 2) 
nbad = nbad + 1; 
k = kel; 
P = PO; % reset P 
else 
% COMPUTATION OF UPDATED ESTIMATE 
Mee(;k+1) = xte(: ,k+1) + G*resid; 


6) ERROR ELLIPSOID PLOTTING ROUTINE 
M = [xte(1,k+1) xte(3,k+1)]'; 
oor ( dlr ( 13) 

eee Co) 
Psy) — eure llip(M kK, .00,25,0); 
eex(: ,k) pao, 
eey(: ,k) = yp; 


% PREDICTED (FUTURE TRACK) USING KALMAN ESTIMATE 
7 (number of predictions desired input from marine.m) 
peredCs, |) ="xte(: ,k+1); 
ace = zeros( 2,1): 
for 1 = 1: kpred % kpred is # of predictions 
if k >= 5 
accx = (xte(2,k+1)-xte(2,k))/dt; 
accy (xte(4,k+1)-xte(4,k))/dt; 
dec, = [ accx,accy)}: 
diwaccx 7= 5 
break 
acc = zeros(2,1); 
elseif accy >= 5 
break 
acc = zeros(2,1); 
end 
end 
Pimed@ lai phi*tpred(: jl )+del-acc: 


1S 


end 
nbad = QO; 
end 
% PLOT OBSERVED AND ESTIMATED TRACKS (TARGET AND OBSERVER) 
plot(xfol(1,1:k+1) ,xfo1(3,1: k+1), xw ,xfoz(1, 1: keane 
xfo2(3,1:k+1),'xw',... 
xto(1,1:k+1),xto(3,1:k+1),'.b ,xte(1l,i2k+1),xte(3yi- epee to yee 
zpos(1,1:k+1),zpos(2,1:k+1),' ow’ ,tpred(1,:),tpred(3,:),'*2', 
eex,eey,'-r'),grid 
title( 'ACTUAL/OBSERVED/ESTIMATED/PREDICTED TGT TRKS 
USING BRG/RNG OBS'),... 
xlabel('DISTANCE X'),ylabel('DISTANCE Y') 
end 
diary off 
meta brgrng 


% JOHN A. HUCKS II 

7 FILE NAME : DOPPLER 

% EXTENDED KALMAN FILTER PROGRAM USING A DOPPLER SHIFTED FREQUENCY 
7% SIGNAL AND THE BEARING TO THAT SIGNAL. 


70 THIS PROGRAM SIMULATES THE TRACK GENERATED BY AN EXTENDED KALMAN 


% FILTER, WITH INPUTS FROM A FUSED SENSOR ARRAY AS IT TRACKS A TARGET 
vo ACROSS AN X-Y GRID SPACE (PLRS FLAT EARTH PROJECTION). 


ele 

clear eex eey xto 

!erase doppler. met 

diary doppler 

% STATE EQUATIONS 

% OBSERVATION TIME INTERVAL (input from marine. m) 


% STATE TRANSITION MATRIX 


phi = (71 deaeee. eee 
0 8) 0 0 
0 Or ou rae 6 
0 O O 1 0 
oe 2 a ae 
% SYSTEM NOISE COEFF ICIENTEMA Rraex 
dei!= [ (des eedierdn 20 0 0 
0 at 0 0 0 
0 0 dt- dt dt 20 
0 0 0 dt 0 
0 0 0 0 dina 


% STATE VARIABLES, FILTER ESTIMATES, TIME, AND OBSERVED DATA 


% NUMBER OF OBSERVATIONS (input from marine. m) 
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%o 
ee 


oO 
fo 


% 

xO 
xfo 
xfO 


% 


6 
fam 


end 


or 
/o 


ESTIMATED TARGET STATE 
= zeros(5,kmax); % initial est. target state 


OBSERVER (SENSOR) INITIAL POSITIONS AND VELOCITIES 
(input from marine. m) 

a = zeros(4,kmax); 

Awan) = Xfol(: ,1); 

1 = xfoa; 


OBSERVER SPEED (input from marine. m) 


INITIAL TIME SETTING 
e = zeros(1,kmax); 


ACTUAL TRACK POSITIONS (input from marine. m) 
zeros(1,kmax); 
zeros(1,kmax); 


0; 

re =] 
ee eel 1), 
yCk) = tgt(4,1); 
fo(k) =tgt(7,1); 

k = 2: kmax 


= I+de- 

ewe Oro tet (3.1) (ia2))t(tet(2, 1). *T)t¢tett 1,1) ); 
eet = tot 3.1 je] )+tet( 2,1); 

y(k) = (0. 5*tgt(6,1)*(Ta2))+(tgt(5,1). *T)+(tgt(4,1)); 


Moles tot Go, 1 )))+tot(5,1); 
fo(k) = tgt(/,1); 
Zoesy— [| 9x 
VX 
y 
Vy 
ey ee % rest frequency xmtr (Hz) 


INPUT OBSERVATION VALUES (BEARING/FREQUENCY ) 
INITIAL OBSERVED TARGET STATE 
xto = zeros(5,kmax); 


meat: i) — | tet(1,1) exe in x direction) 
EeeGl sly) puck ine x direction) 
tgt(4,1) % (km in y direction) 
eee Cored a ~ (kyr in y direetion) 
teeny): % rest frequency xmtr (Hz) 


eel —2pos(.)..: )e- XtO1( 1,2: ); 


deltly Bees 3,7 | =extol(3,: ); 
brgil = atan2(deltlix,deltly); % bearing thtal 
vp = 1.0789e9; % spd light (km/hr) 


for k = i: kmax 
f(k) = (zpos(5,k)*vp)/(vpt((zpos(1,k)*zpos(2,k))+(zpos(3,k)*... 
ZpOs( 44h) ))/saqrt(( zpos(l{k)aZ2Ztzpos(3,,k)a2Z))); 

end 
zobs 


Were l; t); 


xtol = zeros(5,kmax); 


2 


xtolCl,: )*=.zpos(l ar 
Xtol(Z,: =" zpos(2,-3 
XtOl(S.:)) = Zposte= a6 
/ xtol(4,: ) = zpos(asear 
Xtol(532))-= zpos(S-er 
xtol( sa = xtolee ee 


XEO = Xtol: 


% STATE EXCITATION AND MEASUREMENT ERROR COVARIANCE MATRICES 


varv = 1.005; % variance of linear acceleration 
varth = 5.05; % variance of angular acceleration 
vfreq = 1.0; % variance of the rest frequency 


R =f[ le-11 0 
Giese) 1 |]; *% measurement noise 


% INPUT OBSERVATION VALUES WITH NOISE ADDED 
rand( ‘normal’ ) 
zobs = zobs + sqrt(R(1,1))*rand(zobs); 


% INITIAL KALMAN MATRICES AND OBSERVATION 


I = eye(5); 

% INITIAL KALMAN P MATRIX 

PO = [ 3007. 0 0 Be Ueee * reset value of Error Covariance 
0 500° <0 Oigeev 


0 0 300 O 0 
0 0 0 300 0 
0 0 0 0 300 ]; 


*% REINITPARIZING TES IAtTh i 


P =3E0; 
% KALMAN ES @IIATE 
xteC: . 1) eee ale) % (km - x direction) 
iO aeale) fo OM ate) 
xEOC Sa) % (km - y direction) 
Peres) Vomit Vii) 
berg? io) |e % rest frequency xmtr (Hz)_ 


tpred = xte(: ,1); 


% SIMULATION OF KALMAN FILTER OPERATION 
% NUMBER OF BAD OBSERVATIONS 


nbad = 0; % initial setting bad obs. cntr. 
% SIMULATION COUNTER LOOP 
k = 0; 
for k1l=1: 10*kmax 

k=k + 1; % actual loop counter 

if (k == kmax) % check for termination 

break 
end 


% UPDATE FORWARD OBSERVER'S/ FAC'S POSITION (IF THEY ARE MOVING) 
if k <= 3 
way = pi/2; 
else 
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way = =(3%pi/4); 


end 

if spd == 0 
xeolC) kt le=— xtouel.,k): 
meol(3,Ktiye— xtols,k); 

else 
xfol(1,k+1) = xfol(1,k) + spd*sin(way)*dt; 
Xfo1l(2,k+1) = spd*sin(way); 
xfol(3,k+1) = xfol(3,k) + spd*cos(way)*dt; 
xfol(4,k+1) = spd*cos(way); 

end 


% PROJECT AHEAD KALMAN STATE ESTIMATE 
Moe@a,kt]) = phacxte(s 3k). 


Reel DALE 0 

Wiseeresciet( (xteq2, k+l ))a2 + (xte(4,ktl1))a2); 

Ssigxdd2 = ((xte(2,k+1)/vt)a2*varv) + (Cxte(4,k+1)a2)*varth); 
sigydd2 ((xte(4,k+1)/vt)a2*varv) + ((xte(2,k+1)a2)*varth); 
Sigxydd = (xte(2,k+1)*xte(4,k+1))*((varv/(vt*vt))-varth); 
qll = (Cdt*dt)/2).2*sigxdd2; 


ql2 = ((Cdt*dt*dt)/2)*sigxdd2; 
coloe—— ((dedt )/2)“*sigxydd; 
quee——(Cdt-dt-dt )/2)*sigxydd; 
q15 = QO; 

Ge = giz; 

g22 = (de-dt)*sigxdd2; 

g@ezsa — (Cdt<dt=dt )/2)*sigxydd; 
q24 = (dt*dt)*sigxydd; 

gz> = 0: 

Gel = gl3; 

Ge2 = q23; 

q33 = ((dt*dt)/2),2*sigydd2; 
q34 = ((Cdt*dt*dt)/2)*sigydd2; 
q35 = 0; 

q41 = ql4; 

q42 = q24; 

q43 = q34; 

q44 = (dt*dt)*sigydd2; 

q45 = 0; 

gol = qi5; 

pa” eae 

Geo — 9393 

q54 = q45; 

q55 = (dt*dt)*vfreq; 


Oie= ft gall gl2eqi3s qis qi5 
Gel G22g25 1G ceng75 
gol g32 033° g34 q35 
q41 q42 g43 q&4& q45 
God g52 Goo -q54.q55. |; 


Q = del*Q1*(del'); 


% PROJECT ERROR COVARIANCE 
P = phi*P*(phi') + Q; 
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% UPDATE H 

rl2 = sqrt((xte(],kt1)a2)+(xte(3,kt1) 22 

r32 = (sqrt((xte(1,k+1)a2 )+(xte(3 kt: 

deltxvy = xte(1,k+1)*xte(4,k+1) - xte(3,k+1)*xte(2,k+1); 
deltyvx = xte(3,k+1)*xte(2,k+1) - xte(1,k+1)*xte(4,k+1); 
fkvp = xte(5,k+1)*vp; 


hll = -(xte(3,k+1)/CCxte(1,kt]l)a 2)4+CxteC3,k+1)a2))); 
hi2 = ¢; 

hl13 = xte(1,k+1)/((xte(1,k+1)a2)+(xte(3,k+1)a2)); 
hl4 = 0; 

hl5 = 0; 

h21 = -((zobs(2,k+1),2)/fkvp)*(xte(3,k+1)*deltyvx)/r32; 
h22 = -((zobs(2,k+1),2)/fkvp)*(xte(1,k+1)/ri2); 

h23 = -(zobs(2,k+1)/fkvp)*(xte(1,k+1)*deltxvy) /r32; 
h24 = -((zobs(2,k+1)a2)/fkvp)*(xte(3,k+1)/r12); 

h25 = zobs(2,k+1)/xte(5,k+1); 

H = [ @hiT nl2 hls hia 


hn2) h22h2Z3 5h es 


% GET NEW MEASURED ESTIMATE 

deltix”= xte(l,kt+1)3- xfolgl kh): 

deltiy = xte(3,k+))e= xtol@s.rtl), 

zhatl = atan2(deltix,deltly); % bearing estimate thtal 
zhat2 = (xte(5,k+1)*vp)/Cvpt((xte(1,k+1)* % frequency estimate 
xte(2 kt ste eee 

xte(4,ktL) ))/ saunter). k+1)a2+xte(3, k+l) 290))- 


Zhat = [ezheel 
Zac Zane 
% COMPUTATION OF KALMAN GAIN 
vresid = (H*P*(H') + R); % variance of the residual 
G = P*(H') / vresid; % Kalman gain 


% COMPUTATION OF UPDATED ERROR COVARIANCE 
P= (J - GH) P; 


%  COMPUTARIGN SO RESIDUAL 
resid = zobs(: ,k+1) - zhat; 


% IF RESIDUAL IS TOO BIG, THEN RESET P AND DO CALCULATIONS AGAIN. 
if (abs(resid(l)) > 1:0*sdare(abs(vresidQl a) ae 
} abs(resid(2)) > 1.0*sqrt(abs(vresid(2,2)))... 
& nbad = 0) 
nbad = 1; 
k = k-l; 
P = PQ; % reset P 
else 


% COMPUTATION OF UPDATED ESTIMATE 
xte(: ,k+1) = xte(: ,k+1) + G*resid; 


RROR ELLIPSOID PLOTTING ROUTINE 
[xteCl [kt] xteCs ki) 


E 
M = 
K = [ P(1,1) P(1,3) 
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Feo OE Sin ee lit 
xpyp) = erre ltteer 60, 25,0): 
eexG: sk) Dp; 
eey(: ,k) hoy 


% PREDICTED (FUTURE TRACK) USING KALMAN ESTIMATE 
iored(: | )*="xte(:,kt+1); 
aee, = Zeros( 5,1); 
for 1 = 1: kmax 
if@e>= 3 
aiec(C2 1) = (xte(2. kt] )-xte(Z.k))/dt; 
acc(4,1) = (xte(4,k+1)-xte(4,k))/dt; 
ttracce@e 1) >= .5 
break 
acc = zeros(5,1); 
elseif acc(4,1) >= 5 


break 
acc = zeros(5,1); 
end 
end 
tpred(: ,1+1) = phi*tpred(: ,1)+del*acc; 
end 
nbad = 0; 
end 


Pee COP-OBSERVED AND ESTIMATED TRACKS (TARGET AND OBSERVER) 
Pelee ee to1C 1. ik ae xfol(3,1:k+1),'xw' ,xto(1,1:k+1), 
beots, i: kt+1))'. ot 
Meee tok) ),xte(3, 1: Me W »zpos(1, 1: Kol eZ OS Goel k+1),'0 
tpred(1,: a tpred(3,: y, “e ,eex,ecy, -r ),grid 
Eirele( ACTUAL / OBSERVED /ESTIMATED / PREDICTED TGT TRKS 
USING DOPPLER oe - 
xlabel( ‘DISTANCE X'),ylabel( ‘DISTANCE Y') 
end 
meta doppler 


Eunetion Pe — terre lip. k, Pr.) 

% errellip(M,K,Pr,n,MD) 

70 This function takes the following arguments: 
% 

% M Mean Vector (2 x 1) 

% K Covartance Matrix (2 x 2) 

% Pr Peopabtiaty=(0 2. 1) 

vA n Number of points to compute 

% MD Compute by Mahalanobis Distance vice probability 
% QO = Probability 

* 1 = Mahalanobis Distance 


32 


and returns x and y vectors of the confidence ellipse for the 
% given probability or Mahalanobis Distance. 


% Stephen L. Spehn ia) Noy 1969 


wo 


if nargin = 5 


error('Incorrect number of arguments to errellip.'); 


end; 
if MD = 1 

c = abs(Pr); 
else 


Pr = max(min(.995,Pr),.005); 


% Cubic spline fit for the ellipse constant. 


% Using this method is a compromise between speed and accuracy. 
ad ey sae 


pp = { 12; «005; . 01s 79025; 2 OSes eee), 


95559752. 99. oS ae 


4.190809197869072e+1; 4. 
. 15930349191602; 

. 24375856514019e+1; 
.087990965023806e+5; 
.47585746879301le-1; 
. 140048306271903; 


3. 970929262330003; 
1. 62882241111034e+1; 
4. 176985755223079e+2; 
-3. 810356328008524e-1; 
.444089169224142e-1; 


. 6070422925247et2; 
JO02Z06574/5664557— 
212265223099 s05a. 
-66236 1 [40a tae: 

, 382133 2656986/ send 
.O1; . 0201, 2 0506 menus: 
1. 393220772 Gola ero. 
c = ppval(pp,Pr); 

end; 


WMO ND Fh UY 


% Get Eigenvectors, Eigenvalues, and translated 


[Evec,Eval] = eig(K); 
Sree mal Te tie 
SQrt(Eval( 242) ). 


Z 
8 
Ji 
2 
1 
. 55841940780766; I, 
4 
2 
Z 
8 
6 


190809197872625e41; 


477458749113522e+1; 


.920316224166436et+2; 
.02019022643493; 
EZOTO/ S09 ZIT 772 
PII5 632565559641. 
176397289507 1456er1; 


.211; .575; 


feo OC es 


% Parameterized ellipse equations 


t = 09( 2* pi me C2. ae): 
XV = sigx*c*cos(t); 
yv = sigy*c*sin(t); 


Le 
a 


function y = reshape(x,m,n) 


% translate back to the original coordinates 
(xv*Evec(1,1) + yv*Evec(1,2) + M(1))'; 
(xv*Evec(2,1) + yv*Evec(2,2) + M(2))'; 


2.118721291999464e+1; 
539957957 350473 19e—4, 
2. 725551521454689e+3; 
2.087990965023842e+5; 
2. 133449885921905; 

7 
5 
9 
2 
Zz 
il 
2 


111734877634124: 


» 187150103426589e+1; 
»660/9909650227>/en5: 
- 055905 760326949; 

- 694842569743673; 
-819254614465004e+1; 
- 23340067762321e+2; 


variances 


*RESHAPE RESHAPE(X,M,N) returns the M-by-N matrix whose elements 


%are taken columnwise from X. 


*not have M*N elements. 
mm,nn = size(x); 

if mm*nn = m*n 
error('’Matrix must have M*N 
end 


elements. ') 


80 


An error results if X does 


y = zeros(m,n); 
Me = X; 


8 1 
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